We propose a topological mapping and localization system able to operate on real human colonoscopies, despite significant shape and illumination changes. The map is a graph where each node codes a colon location by a set of real images, while edges represent traversability between nodes. For close-in-time images, where scene changes are minor, place recognition can be successfully managed with the recent transformers-based local feature matching algorithms. However, under long-term changes –such as different colonoscopies of the same patient– feature-based matching fails. To address this, we train on real colonoscopies a deep global descriptor achieving high recall with significant changes in the scene. The addition of a Bayesian filter boosts the accuracy of long-term place recognition, enabling relocalization in a previously built map. Our experiments show that ColonMapper is able to autonomously build a map and localize against it in two important use cases: localization within the same colonoscopy or within different colonoscopies of the same patient. Code: github.com/jmorlana/ColonMapper.

This article presents NR-SLAM, a novel nonrigid monocular simultaneous localization and mapping (SLAM) system founded on the combination of a dynamic deformation graph with a visco-elastic deformation model. The former enables our system to represent the dynamics of the deforming environment as the camera explores, while the later allows us to model general deformations in a simple way. The presented system is able to automatically initialize and extend a map modeled by a sparse point cloud in deforming environments, that is refined with a sliding-window deformable bundle adjustment. This map serves as base for the estimation of the camera motion and deformation and enables us to represent arbitrary surface topologies, overcoming the limitations of previous methods. To assess the performance of our system in challenging deforming scenarios, we evaluate it in several representative medical datasets. In our experiments, NR-SLAM outperforms previous deformable SLAM systems, achieving millimeter reconstruction accuracy and bringing automated medical intervention closer. For the benefit of the community, we make the source code public.

Single-view depth estimation can be remarkably effective if there is enough ground-truth depth data for supervised training. However, there are scenarios, especially in medicine in the case of endoscopies, where such data cannot be obtained. In such cases, multi-view self-supervision and synthetic-to-real transfer serve as alternative approaches, however, with a considerable performance reduction in comparison to supervised case. Instead, we propose a single-view self-supervised method that achieves a performance similar to the supervised case. In some medical devices, such as endoscopes, the camera and light sources are co-located at a small distance from the target surfaces. Thus, we can exploit that, for any given albedo and surface orientation, pixel brightness is inversely proportional to the square of the distance to the surface, providing a strong single-view self-supervisory signal. In our experiments, our self-supervised models deliver accuracies comparable to those of fully supervised ones, while being applicable without depth ground-truth data.

Visual SLAM inside the human body will open the way to computer-assisted navigation in endoscopy. However, due to space limitations, medical endoscopes only provide monocular images, leading to systems lacking true scale. In this paper, we exploit the controlled lighting in colonoscopy to achieve the first in-vivo 3D reconstruction of the human colon using photometric stereo on a calibrated monocular endoscope. Our method works in a real medical environment, providing both a suitable in-place calibration procedure and a depth estimation technique adapted to the colon's tubular geometry. We validate our method on simulated colonoscopies, obtaining a mean error of 7% on depth estimation, which is below 3 mm on average. Our qualitative results on the EndoMapper dataset show that the method is able to correctly estimate the colon shape in real human colonoscopies, paving the ground for truescale monocular SLAM in endoscopy.

We present a generic framework for scale-aware direct monocular odometry based on depth prediction from a deep neural network. In contrast with previous methods where depth information is only partially exploited, we formulate a novel depth prediction residual which allows us to incorporate multi-view depth information. In addition, we propose to use a truncated robust cost function which prevents considering inconsistent depth estimations. The photometric and depth-prediction measurements are integrated into a tightly-coupled optimization leading to a scale-aware monocular system which does not accumulate scale drift. Our proposal does not particularize for a concrete neural network, being able to work along with the vast majority of the existing depth prediction solutions. We demonstrate the validity and generality of our proposal evaluating it on the KITTI odometry dataset, using two publicly available neural networks and comparing it with similar approaches and the state-of-the-art for monocular and stereo SLAM. Experiments show that our proposal largely outperforms classic monocular SLAM, being 5 to 9 times more precise, beating similar approaches and having an accuracy which is closer to that of stereo systems.

Monocular SLAM in deformable scenes will open the way to multiple medical applications like computer-assisted navigation in endoscopy, automatic drug delivery or autonomous robotic surgery. In this paper we propose a novel method to simultaneously track the camera pose and the 3D scene deformation, without any assumption about environment topology or shape. The method uses an illumination-invariant photometric method to track image features and estimates camera motion and deformation combining reprojection error with spatial and temporal regularization of deformations. Our results in simulated colonoscopies show the method's accuracy and robustness in complex scenes under increasing levels of deformation. Our qualitative results in human colonoscopies from Endomapper dataset show that the method is able to successfully cope with the challenges of real endoscopies: deformations, low texture and strong illumination changes. We also compare with previous tracking methods in simpler scenarios from Hamlyn dataset where we obtain competitive performance, without needing any topological assumption.

Conventional SLAM techniques strongly rely on scene rigidity to solve data association, ignoring dynamic parts of the scene. In this work we present Semi-Direct DefSLAM (SD-DefSLAM), a novel monocular deformable SLAM method able to map highly deforming environments, built on top of DefSLAM [1]. To robustly solve data association in challenging deforming scenes, SD-DefSLAM combines direct and indirect methods: an enhanced illumination-invariant Lucas-Kanade tracker for data association, geometric Bundle Adjustment for pose and deformable map estimation, and bag-of-words based on feature descriptors for camera relocalization. Dynamic objects are detected and segmented-out using a CNN trained for the specific application domain.We thoroughly evaluate our system in two public datasets. The mandala dataset is a SLAM benchmark with increasingly aggressive deformations. The Hamlyn dataset contains intracorporeal sequences that pose serious real-life challenges beyond deformation like weak texture, specular reflections, surgical tools and occlusions. Our results show that SD-DefSLAM outperforms DefSLAM in point tracking, reconstruction accuracy and scale drift thanks to the improvement in all the data association steps, being the first system able to robustly perform SLAM inside the human body.

This article presents ORB-SLAM3, the first system able to perform visual, visual-inertial and multimap SLAM with monocular, stereo and RGB-D cameras, using pin-hole and fisheye lens models. The first main novelty is a tightly integrated visual-inertial SLAM system that fully relies on maximuma posteriori(MAP) estimation, even during IMU initialization, resulting in real-time robust operation in small and large, indoor and outdoor environments, being two to ten times more accurate than previous approaches. The second main novelty is a multiple map system relying on a new place recognition method with improved recall that lets ORB-SLAM3 survive to long periods of poor visual information: when it gets lost, it starts a new map that will be seamlessly merged with previous maps when revisiting them. Compared with visual odometry systems that only use information from the last few seconds, ORB-SLAM3 is the first system able to reuse in all the algorithm stages all previous information from high parallax co-visible keyframes, even if they are widely separated in time or come from previous mapping sessions, boosting accuracy. Our experiments show that, in all sensor configurations, ORB-SLAM3 is as robust as the best systems available in the literature and significantly more accurate. Notably, our stereo-inertial SLAM achieves an average accuracy of 3.5 cm in the EuRoC drone and 9 mm under quick hand-held motions in the room of TUM-VI dataset, representative of AR/VR scenarios. For the benefit of the community we make public the source code.

We formulate for the first time visual-inertial initialization as an optimal estimation problem, in the sense of maximum-a-posteriori (MAP) estimation. This allows us to properly take into account IMU measurement uncertainty, which was neglected in previous methods that either solved sets of algebraic equations, or minimized ad-hoc cost functions using least squares. Our exhaustive initialization tests on EuRoC dataset show that our proposal largely outperforms the best methods in the literature, being able to initialize in less than 4 seconds in almost any point of the trajectory, with a scale error of 5.3% on average. This initialization has been integrated into ORB-SLAM Visual-Inertial boosting its robustness and efficiency while maintaining its excellent accuracy.

Visual-inertial SLAM (VI-SLAM) requires a good initial estimation of the initial velocity, orientation with respect to gravity and gyroscope and accelerometer biases. In this paper we build on the initialization method proposed by Martinelli [1] and extended by Kaiser et al. [2], modifying it to be more general and efficient. We improve accuracy with several rounds of visual-inertial bundle adjustment, and robustify the method with novel observability and consensus tests, that discard erroneous solutions. Our results on the EuRoC dataset show that, while the original method produces scale errors up to 156%, our method is able to consistently initialize in less than two seconds with scale errors around 5%, which can be further reduced to less than 1% performing visual-inertial bundle adjustment after ten seconds.

We propose ORBSLAM-Atlas, a system able to handle an unlimited number of disconnected sub-maps, that includes a robust map merging algorithm able to detect submaps with common regions and seamlessly fuse them. The outstanding robustness and accuracy of ORBSLAM are due to its ability to detect wide-baseline matches between keyframes, and to exploit them by means of non-linear optimization, however it only can handle a single map. ORBSLAM-Atlas brings the wide-baseline matching detection and exploitation to the multiple map arena. The result is a SLAM system significantly more general and robust, able to perform multisession mapping. If tracking is lost during exploration, instead of freezing the map, a new sub-map is launched, and it can be fused with the previous map when common parts are visited. Our criteria to declare the camera lost contrast with previous approaches that simply count the number of tracked points, we propose to discard also inaccurately estimated camera poses due to bad geometrical conditioning. As a result, the map is split into more accurate sub-maps, that are eventually merged in a more accurate global map, thanks to the multi-mapping capabilities.We provide extensive experimental validation in the EuRoC datasets, where ORBSLAM-Atlas obtains accurate monocular and stereo results in the difficult sequences where ORBSLAM failed. We also build global maps after multiple sessions in the same room, obtaining the best results to date, between 2 and 3 times more accurate than competing multi-map approaches. We also show the robustness and capability of our system to deal with dynamic scenes, quantitatively in the EuRoC datasets and qualitatively in a densely populated corridor where camera occlusions and tracking losses are frequent.

We present ORB-SLAM2, a complete simultaneous localization and mapping (SLAM) system for monocular, stereo and RGB-D cameras, including map reuse, loop closing, and relocalization capabilities. The system works in real time on standard central processing units in a wide variety of environments from small hand-held indoors sequences, to drones flying in industrial environments and cars driving around a city. Our back-end, based on bundle adjustment with monocular and stereo observations, allows for accurate trajectory estimation with metric scale. Our system includes a lightweight localization mode that leverages visual odometry tracks for unmapped regions and matches with map points that allow for zero-drift localization. The evaluation on 29 popular public sequences shows that our method achieves state-of-the-art accuracy, being in most cases the most accurate SLAM solution. We publish the source code, not only for the benefit of the SLAM community, but with the aim of being an out-of-the-box SLAM solution for researchers in other fields.

The vision of the RoboEarth project is to design a knowledge-based system to provide web and cloud services that can transform a simple robot into an intelligent one. In this work, we describe the RoboEarth semantic mapping system. The semantic map is composed of: 1) an ontology to code the concepts and relations in maps and objects and 2) a SLAM map providing the scene geometry and the object locations with respect to the robot. We propose to ground the terminological knowledge in the robot perceptions by means of the SLAM map of objects. RoboEarth boosts mapping by providing: 1) a subdatabase of object models relevant for the task at hand, obtained by semantic reasoning, which improves recognition by reducing computation and the false positive rate; 2) the sharing of semantic maps between robots; and 3) software as a service to externalize in the cloud the more intensive mapping computations, while meeting the mandatory hard real time constraints of the robot. To demonstrate the RoboEarth cloud mapping system, we investigate two action recipes that embody semantic map building in a simple mobile robot. The first recipe enables semantic map building for a novel environment while exploiting available prior information about the environment. The second recipe searches for a novel object, with the efficiency boosted thanks to the reasoning on a semantically annotated map. Our experimental results demonstrate that, by using RoboEarth cloud services, a simple robot can reliably and efficiently build the semantic maps needed to perform its quotidian tasks. In addition, we show the synergetic relation of the SLAM map of objects that grounds the terminological knowledge coded in the ontology.

This paper presents ORB-SLAM, a feature-based monocular simultaneous localization and mapping (SLAM) system that operates in real time, in small and large indoor and outdoor environments. The system is robust to severe motion clutter, allows wide baseline loop closing and relocalization, and includes full automatic initialization. Building on excellent algorithms of recent years, we designed from scratch a novel system that uses the same features for all SLAM tasks: tracking, mapping, relocalization, and loop closing. A survival of the fittest strategy that selects the points and keyframes of the reconstruction leads to excellent robustness and generates a compact and trackable map that only grows if the scene content changes, allowing lifelong operation. We present an exhaustive evaluation in 27 sequences from the most popular datasets. ORB-SLAM achieves unprecedented performance with respect to other state-of-the-art monocular SLAM approaches. For the benefit of the community, we make the source code public.

In this paper we present for the first time a relocalisation method for keyframe-based SLAM that can deal with severe viewpoint change, at frame-rate, in maps containing thousands of keyframes. As this method relies on local features, it permits the interoperability between cameras, allowing a camera to relocalise in a map built by a different camera. We also perform loop closing (detection + correction), at keyframerate, in loops containing hundreds of keyframes. For both relocalisation and loop closing, we propose a bag of words place recognizer with ORB features, which is able to recognize places spending less than 39 ms, including feature extraction, in databases containing 10K images (without geometrical verification). We evaluate the performance of this recognizer in four different datasets, achieving high recall and no false matches, and getting better results than the state-of-art in place recognition, being one order of magnitude faster.

We propose a novel method for visual place recognition using bag of words obtained from accelerated segment test (FAST)+BRIEF features. For the first time, we build a vocabulary tree that discretizes a binary descriptor space and use the tree to speed up correspondences for geometrical verification. We present competitive results with no false positives in very different datasets, using exactly the same vocabulary and settings. The whole technique, including feature extraction, requires 22 ms/frame in a sequence with 26 300 images that is one order of magnitude faster than previous approaches.

We propose a place recognition algorithm for simultaneous localization and mapping (SLAM) systems using stereo cameras that considers both appearance and geometric information of points of interest in the images. Both near and far scene points provide information for the recognition process. Hypotheses about loop closings are generated using a fast appearance-only technique based on the bag-of-words (BoW) method. We propose several important improvements to BoWs that profit from the fact that, in this problem, images are provided in sequence. Loop closing candidates are evaluated using a novel normalized similarity score that measures similarity in the context of recent images in the sequence. In cases where similarity is not sufficiently clear, loop closing verification is carried out using a method based on conditional random fields (CRFs). We build on CRF matching with two main novelties: We use both image and 3-D geometric information, and we carry out inference on a minimum spanning tree (MST), instead of a densely connected graph. Our results show that MSTs provide an adequate representation of the problem, with the additional advantages that exact inference is possible and that the computational cost of the inference process is limited. We compare our system with the state of the art using visual indoor and outdoor data from three different locations and show that our system can attain at least full precision (no false positives) for a higher recall (fewer false negatives).

Monocular SLAM systems have been mainly focused on producing geometric maps just composed of points or edges; but without any associated meaning or semantic content. In this paper, we propose a semantic SLAM algorithm that merges in the estimated map traditional meaningless points with known objects. The non-annotated map is built using only the information extracted from a monocular image sequence. The known object models are automatically computed from a sparse set of images gathered by cameras that may be different from the SLAM camera. The models include both visual appearance and tridimensional information. The semantic or annotated part of the map -the objects- are estimated using the information in the image sequence and the precomputed object models. The proposed algorithm runs an EKF monocular SLAM parallel to an object recognition thread. This latest one informs of the presence of an object in the sequence by searching for SURF correspondences and checking afterwards their geometric compatibility. When an object is recognized it is inserted in the SLAM map, being its position measured and hence refined by the SLAM algorithm in subsequent frames. Experimental results show real-time performance for a hand held camera imaging a desktop environment and for a camera mounted in a robot moving in a room-sized scenario.

We present a method for detecting revisited places in a image sequence in real time by using efficient features. We introduce three important novelties to the bag-of-words plus geometrical checking approach. We use FAST keypoints and BRIEF descriptors, which are binary and very fast to compute (less that 20µs per point). To perform image comparisons, we make use of a bag of words that discretises the binary descriptor space and an inverse index. We also introduce the use of a direct index to take advantage of the bag of words to obtain correspondence points between two images efficiently, avoiding a matching of complexity Θ(n2). To detect loop closure candidates, we propose managing matches in groups to increase the reliability of the candidates returned by the bag of words. We present results in three real and public datasets, with 0.7–1.7 Km long trajectories. We obtain high precision and recall rates, spending 16 ms on average per image for the feature computation and the whole loop detection process in sequences with 19000 images, one order of magnitude less than other similar techniques.

Place recognition is a challenging task in any SLAM system. Algorithms based on visual appearance are becoming popular to detect locations already visited, also known as loop closures, because cameras are easily available and provide rich scene detail. These algorithms typically result in pairs of images considered depicting the same location. To avoid mismatches, most of them rely on epipolar geometry to check spatial consistency. In this paper we present an alternative system that makes use of stereo vision and combines two complementary techniques: bag-of-words to detect loop closing candidate images, and conditional random fields to discard those which are not geometrically consistent. We evaluate this system in public indoor and outdoor datasets from the Rawseeds project, with hundred-metre long trajectories. Our system achieves more robust results than using spatial consistency based on epipolar geometry.

In this paper we describe an algorithm to compute cycle constraints that can be used in many graph-based SLAM algorithms; we exemplify it in hierarchical SLAM. Our algorithm incrementally computes the minimum cycle basis of constraints from which any other cycle can be derived. Cycles in this basis are local and of minimum length, so that the associated cycle constraints have less linearization problems. This also permits to construct regional maps, that is, it makes possible efficient and accurate intermediate mapping levels between local maps and the whole global map. We have extended our algorithm to the multi-robot case. We have tested our methodology using the Victoria Park data set with satisfactory results.

When solving the Simultaneous Localization and Mapping (SLAM) problem, submapping and graphical methods have shown to be valuable approaches that provide significant advantages over the standard EKF solution: they are faster and can produce more consistent estimates when using local coordinates. In this paper we present CI-Graph, a submapping method for SLAM that uses a graph structure to efficiently solve complex trajectories reducing the computational cost. Unlike other submapping SLAM approaches, we are able to transmit and share information through maps in the graph in a consistent manner by using conditionally independent submaps. In addition, the current submap always summarizes, without further computations, all information available making CI-Graph be an intrinsically “up to date” algorithm. Moreover, the technique is also efficient in memory requirements since it does not need to recover the full covariance matrix. To evaluate CI-Graph performance, the method has been tested using a synthetic Manhattan world and Victoria Park data set.

In this paper, we address the problem of creating an objective benchmark for comparing SLAM approaches. We propose a framework for analyzing the results of SLAM approaches based on a metric for measuring the error of the corrected trajectory. The metric uses only relative relations between poses and does not rely on a global reference frame. The idea is related to graph-based SLAM approaches in the sense that it considers the energy needed to deform the trajectory estimated by a SLAM approach to the ground truth trajectory. Our method enables us to compare SLAM approaches that use different estimation techniques or different sensor modalities since all computations are made based on the corrected trajectory of the robot. We provide sets of relative relations needed to compute our metric for an extensive set of datasets frequently used in the SLAM community. The relations have been obtained by manually matching laser-range observations. We believe that our benchmarking framework allows the user an easy analysis and objective comparisons between different SLAM approaches.

In this paper we present a loop closure method for a handheld single-camera SLAM system based on our previous work on relocalization. By finding correspondences between the current image and the map, our system is able to reliably detect loop closures. We compare our algorithm to existing techniques for loop closure in single-camera SLAM based on both image-to-image and map-to-map correspondences and discuss both the reliability and suitability of each algorithm in the context of monocular SLAM.

In this paper, we describe a system that can carry out simultaneous localization and mapping (SLAM) in large indoor and outdoor environments using a stereo pair moving with 6 DOF as the only sensor. Unlike current visual SLAM systems that use either bearing-only monocular information or 3-D stereo information, our system accommodates both monocular and stereo. Textured point features are extracted from the images and stored as 3-D points if seen in both images with sufficient disparity, or stored as inverse depth points otherwise. This allows the system to map both near and far features: the first provide distance and orientation, and the second provide orientation information. Unlike other vision-only SLAM systems, stereo does not suffer from “scale drift” because of unobservability problems, and thus, no other information such as gyroscopes or accelerometers is required in our system. Our SLAM algorithm generates sequences of conditionally independent local maps that can share information related to the camera motion and common features being tracked. The system computes the full map using the novel conditionally independent divide and conquer algorithm, which allows constant time operation most of the time, with linear time updates to compute the full map. To demonstrate the robustness and scalability of our system, we show experimental results in indoor andoutdoor urban environments of 210 m and 140 m loop trajectories, with the stereo camera being carried in hand by a person walking at normal walking speeds of 4--5 km/h.

In this paper, we show thatallprocesses associated with the move-sense-update cycle of extended Kalman filter (EKF) Simultaneous Localization and Mapping (SLAM) can be carried out in timelinearwith the number of map features. We describeDivide and ConquerSLAM, which is an EKF SLAM algorithm in which the computational complexity per step is reduced from$O(n^2)$to$O(n)$, and the total cost of SLAM is reduced from$O(n^3)$to$O(n^2)$. Unlike many current large-scale EKF SLAM techniques, this algorithm computes a solution without relying on approximations or simplifications (other than linearizations) to reduce computational complexity. Also, estimates and covariances are available when needed by data association without any further computation. Furthermore, as the method works most of the time in local maps, where angular errors remain small, the effect of linearization errors is limited. The resulting vehicle and map estimates are more precise than those obtained with standard EKF SLAM. The errors with respect to the true value are smaller, and the computed state covariance is consistent with the real error in the estimation. Both simulated experiments and the Victoria Park dataset are used to provide evidence of the advantages of this algorithm.

Simultaneous localization and mapping (SLAM) algorithms based on local maps have been demonstrated to be well suited for mapping large environments as they reduce the computational cost and improve the consistency of the final estimation. The main contribution of this paper is a novel submapping technique that does not require independence between maps. The technique is based on the intrinsic structure of the SLAM problem that allows the building of submaps that can share information, remainingconditionally independent. The resulting algorithm obtains local maps in constant time during the exploration of new terrain and recovers the global map in linear time after simple loop closures without introducing any approximations besides the inherent extended Kalman filter linearizations. The memory requirements are also linear with the size of the map. As the algorithm works in a covariance form, well-known data-association techniques can be used in the usual manner. We present experimental results using a handheld monocular camera, building a map along a closed-loop trajectory of 140 m in a public square, with people and other clutter. Our results show that the combination of conditional independence, which enables the system to share the camera and feature states between submaps, and local coordinates, which reduce the effects of linearization errors, allow us to obtain precise maps of large areas with pure monocular SLAM in real time.

In this paper we describe divide and conquer SLAM (D&C SLAM), an algorithm for performing simultaneous localization and mapping using the extended Kalman filter. D&C SLAM overcomes the two fundamental limitations of standard EKF SLAM: 1.) the computational cost per step is reduced from O(n2) to O(n) (the cost full SLAM is reduced from O(n3) to O(n2)); 2.) the resulting vehicle and map estimates have better consistency properties than standard EKF SLAM in the sense that the computed state covariance adequately represents the real error in the estimation. Unlike many current large scale EKF SLAM techniques, this algorithm computes an exact solution, without relying on approximations or simplifications to reduce computational complexity. Also, estimates and covariances are available when needed by data association without any further computation. Empirical results show that, as a bi-product of reduced computations, and without losing precision because of approximations, D&C SLAM has better consistency properties than standard EKF SLAM. Both characteristics allow to extend the range of environments that can be mapped in real time using EKF. We describe the algorithm and study its computational cost and consistency properties.

This paper presents the benefits of using a low cost inertial measurement unit to aid in an implementation of inverse depth initialized SLAM using a hand-held monocular camera. Results are presented with and without inertial observations for different assumed initial ranges to features on the same dataset. When using only the camera, the scale of the scene is not observable. As expected, the scale of the map depends on the prior used to initialize the depth of the features and may drift when exploring new terrain, precluding loop closure. The results show that the inertial observations help to improve the estimated trajectory of the camera leading to a better estimation of the map scale and a more accurate localization of features.

Local maps algorithms have demonstrated to be well suited for mapping large environments as can reduce the computational cost and improve the consistency of the final estimation. In this paper we present a new technique that allows the use of local mapping algorithms in the context of EKF SLAM but without the constrain of probabilistic independence between local maps. By means of this procedure, salient features of the environment or vehicle state components as velocity or global attitude, can be shared between local maps without affecting the posterior joining process or introducing any undesirable approximations in the final global map estimate. The overload cost introduced by the technique is minimum since building up local maps does not require any additional operations apart from the usual EKF steps. As the algorithm works with covariance matrices, well-known data association techniques can be used in the usual manner. To test the technique, experimental results using a monocular camera in an outdoor environment are provided. The initialization of the features is based on the inverse depth algorithm.

This paper describes a navigation system for autonomous underwater vehicles (AUVs) in partially structured environments, such as dams, harbors, marinas or marine platforms. A mechanical scanning imaging sonar is used to obtain information about the location of planar structures present in such environments. A modified version of the Hough transform has been developed to extract line features, together with their uncertainty, from the continuous sonar datafiow. The information obtained is incorporated into a feature-based SLAM algorithm running an Extended Kalman Filter (EKF). Simultaneously, the AUV’s position estimate is provided to the feature extraction algorithm to correct the distortions that the vehicle motion produces in the acoustic images. Experiments carried out in a marina located in the Costa Brava (Spain) with the Ictineu AUV show the viability of the proposed approach.

The probability of finding alive a person buried by a snow avalanche decreases dramatically with time. The best chance for the victims is to carry an avalanche beacon or ARVA (from the french: Appareilde Recherche de Victimes d'Avalanche), that transmits a magnetic field that can be detected by the rescuer's ARVA. However, the signals received are difficult to interpret and require people with good training on the actual searching techniques. In this paper we propose to address the search for victims as a SLAM problem: tracking the rescuer location while building a map of the locations of the victims under the snow, using 3D measurements of the magnetic field produced by their ARVAs. Given the high non-linearity of the problem, we propose a technique based on sum of Gaussians (SOGs) and extended Kalman filter. We present preliminary simulation results showing that this technique is faster and more accurate than the classical ones

This paper presents a robust algorithm for segmentation and line detection in 2D range scans. The described method exploits the multimodal probability density function of the residual error. It is capable of segmenting the range data in clusters, estimate the straight segments parameters, and estimate the scale of inliers error noise successfully, despite of high level of spurious data. No prior knowledge about the sensor and object properties is given to the algorithm. The mode seeking is based on mean shift algorithm, which has been widely used and tested in 3D laser scan segmentation, machine learning and pattern recognition applications. We show the reliability of the technique with experimental indoor and outdoor manmade environment. Compared with classical methods, a good compromise between false positive, false negative, wrong segment split and wrong segment merge is achieved, with improved accuracy in the estimated parameters.

A person buried by a snow avalanche can be found by measuring the magnetic field generated by an avalanche beacon or ARVA carried by the victim. However, the signals received are difficult to interpret and require people with good training on the actual searching techniques. In this paper we show that the search can be automated using SLAM techniques. The rescuer is equipped with an inertial sensor to estimate its own motion and a triple antenna to obtain 3D measurements of the magnetic field generated by the victim's ARVA. Both measurements are used to build a "robocentric" map that contains the location of the victim relative to the rescuer. To solve this highly non-linear SLAM problem we propose and compare two alternative solutions based on a sum of Gaussians (SOGs) filter and a particle filter. We present simulation results showing that, for comparable computing times, the SOGs solution gives more accurate results

In this paper we describe a system for underwater navigation with AUVs in partially structured environments, such as dams, ports or marine platforms. An imaging sonar is used to obtain information about the location of planar structures present in such environments. This information is incorporated into a feature-based SLAM algorithm in a two step process: (I) the full 360deg sonar scan is undistorted (to compensate for vehicle motion), thresholded and segmented to determine which measurements correspond to planar environment features and which should be ignored; and (2) SLAM proceeds once the data association is obtained: both the vehicle motion and the measurements whose correct association has been previously determined are incorporated in the SLAM algorithm. This two step delayed SLAM process allows to robustly determine the feature and vehicle locations in the presence of large amounts of spurious or unrelated measurements that might correspond to boats, rocks, etc. Preliminary experiments show the viability of the proposed approach

The work presented in this paper deals with scan matching localization using ultrasonic range sensors. Our contribution resides in the extension of ICP based algorithms to be used with ultrasonic sensor data. This extension consists of a pre-process step, where ultrasonic sensor readings are grouped to overcome their sparseness, and a post-process step, where the whole robot trajectory involved in the grouping process is corrected. Thanks to that a great improvement with respect to odometry is obtained. Experimental results show that even huge odometric errors are corrected with the presented method.

In this paper we study the global localization problem in SLAM: the determination of the vehicle location in a previously mapped environment with no other prior information. We show that, using a grid sampling representation of the configuration space, it is possible to evaluate all vehicle location hypotheses in the environment (up to a certain resolution) with a computational cost that is bilinear: linear both in the number of map features and in the number of sensor measurements. We propose a pairing-driven algorithm that considers only individual measurement-feature pairings and thus, in contrast with current correspondence space algorithms, it avoids searching in the exponential correspondence space. It uses a voting strategy that accumulates evidence for each vehicle location hypothesis, assuring robustness to noise in the sensor measurements and environment models. The general nature of the proposed strategy allows the consideration of different types of features and sensor measurements. Using the popular Victoria Park dataset, we compare its performance with location-driven algorithms where the solution space is usually randomly sampled. We show that the proposed pairing-driven technique is computationally more efficient in proportion to the density of features in the environment.

In this paper, we present a hierarchical mapping method that allows us to obtain accurate metric maps of large environments in real time. The lower (or local) map level is composed of a set of local maps that are guaranteed to be statistically independent. The upper (or global) level is an adjacency graph whose arcs are labeled with the relative location between local maps. An estimation of these relative locations is maintained at this level in a relative stochastic map. We propose a close to optimal loop closing method that, while maintaining independence at the local level, imposes consistency at the global level at a computational cost that is linear with the size of the loop. Experimental results demonstrate the efficiency and precision of the proposed method by mapping the Ada Byron building at our campus. We also analyze, using simulations, the precision and convergence of our method for larger loops.

Abstract — In this paper we propose an algorithm to determine the location of a vehicle in an environment represented by a stochastic map, given a set of environment measurements obtained by a sensor mounted on the vehicle. We show that the combined use of (1) geometric constraints considering feature correlation, (2) joint compatibility, (3) random sampling and (4) locality, make this algorithm linear with both the size of the stochastic map and the number of measurements. We demonstrate the practicality and robustness of our approach with experiments in an outdoor environment. I.

This paper describes a real-time implementation of feature-based concurrent mapping and localization (CML) running on a mobile robot in a dynamic indoor environment. Novel characteristics of this work include: (1) a hierarchical representation of uncertain geometric relationships that extends the SPMap framework, (2) use of robust statistics to perform extraction of line segments from laser data in real-time, and (3) the integration of CML with a "roadmap" path planning method for autonomous trajectory execution. These Innovations are combined to demonstrate the ability for a mobile robot to autonomously return back to its starting position within a few centimeters of precision, despite the presence of numerous people walking through the environment.

This paper describes how multisensor fusion increases both reliability and precision of the environmental observations used for the simultaneous localization and map-building problem for mobile robots. Multisensor fusion is performed at the level of landmarks, which represent sets of related and possibly correlated sensor observations. The work emphasizes the idea of partial redundancy due to the different nature of the information provided by different sensors. Experimentation with a mobile robot equipped with a multisensor system composed of a 2D laser rangefinder and a charge coupled device camera is reported.

In this paper, we address the problem of robust data association for simultaneous vehicle localization and map building. We show that the classical gated nearest neighbor approach, which considers each matching between sensor observations and features independently, ignores the fact that measurement prediction errors are correlated. This leads to easily accepting incorrect matchings when clutter or vehicle errors increase. We propose a new measurement of the joint compatibility of a set of pairings that successfully rejects spurious matchings. We show experimentally that this restrictive criterion can be used to efficiently search for the best solution to data association. Unlike the nearest neighbor, this method provides a robust solution in complex situations, such as cluttered environments or when revisiting previously mapped regions.

We present a comparative study of the performance of map-based robot localisation processes based on diverse sensing devices such as monocular and trinocular vision systems and laser rangefinders. We study both the precision (error with respect to the true values) and robustness (sensor measurements correctly paired with map features) of each localisation process. The experiment design we used allows one to compare these processes under exactly the same conditions. We conclude that comparable precision levels can be attained with each of the three sensors. With respect to robustness, monocular and trinocular vision pose more complex matching problems than laser, requiring more elaborate solutions to make the process robust.

Describes a two-level representation of indoor environments as an intermediate goal towards the topological description of the navigation area in which a mobile robot performs its tasks. Geometric features detected by the exteroceptive sensors of the vehicle are grouped into landmarks, which are characterized by a local reference frame. Each of these landmarks is expressed with respect to a global reference frame. The proposed approach provides a common framework for both the absolute location of the vehicle with respect to the global reference frame, and for its relative location with respect to the landmark frames. A complete representation of the environment is incrementally constructed while the mobile robot is relocalized along its trajectory. A probabilistic representation of uncertain geometric information is used (the SP-model). Experimental results obtained with the mobile robot HILARE-2bis and its 2D laser range finder are presented to validate the approach.

This article describes a rigorous and complete framework for the simultaneous localization and map building problem for mobile robots: the symmetries and perturbation map (SPmap), which is based on a general probabilistic representation of uncertain geometric information. We present a complete experiment with a LabMate/sup TM/ mobile robot navigating in a human-made indoor environment and equipped with a rotating 2D laser rangefinder. Experiments validate the appropriateness of our approach and provide a real measurement of the precision of the algorithms.

We present the two-dimensional (2-D) version of the symmetries and perturbation model (SPmodel), a probabilistic representation model and an extended Kalman filter integration mechanism for uncertain geometric information that is suitable for sensor fusion and integration in multisensor systems. We apply the SPmodel to the problem of location estimation in indoor mobile robotics, experimenting with the mobile robot MACROBE. We have chosen two types of complementary sensory information: (1) range images; (2) intensity images; obtained from a laser sensor. Results of these experiments show that fusing simple and computationally inexpensive sensory information can allow a mobile robot to precisely locate itself. They also demonstrate the generality of the proposed fusion and integration mechanism.

During mobile robot navigation, position estimates obtained by odometry drift with time, therefore becoming unrealistic and useless. This work enhances the use of external mechanisms by considering a multisensor system, composed of a 2D laser rangefinder and an off-the-shelf CCD camera, which provides redundancy and assures reliability and precision of the observed features. We simultaneously consider both the map building and the localization problems using a state vector approach, which is related to the location estimations of both the robot and the map features, whilst its covariance matrix reflects the relationships between them. Relevance and importance of its off-diagonal elements is demonstrated by their contributions to "backwards estimations" whenever the vehicle returns to places in the navigation area which have been already visited and learned. Real experiments are presented, considering a LabMate mobile robot navigating in an static indoor environment.

The work presented in this paper is aimed at evaluating the influence of correlations between map entities on the process of robot relocation and global map building of the environment of a mobile robot navigating in an indoor environment. An EKF filter approach, supported by a probabilistic model to represent uncertain geometric information, is used to process the information obtained by the sensors mounted on the robot. We have developed two approaches, first, considering the existence of correlations, and second assuming independence between entities of the map. We have experimented with the mobile robot MACROBE, using its laser rangefinder.

Presents the 2D version of the symmetries and perturbation model (SPmodel), a probabilistic representation model and an EKF integration mechanism for uncertain geometric information that is suitable for sensor fusion and integration in multisensor systems. The authors apply the SPmodel to the problem of location estimation in mobile robotics, experimenting with the mobile robot MACROBE. The authors have chosen two types of sensorial information whose complementary nature allows a mobile robot to localize itself precisely in a known environment: range images and intensity images obtained from a laser sensor. By measuring distance to objects, range basically provides position information. On the other hand, the intensity image provided by the laser sensor (equivalent to a monocular vision system) can be used to detect vertical edges, corresponding to corners, frames, etc., which mainly provide angular information. Results of these experiments show that fusing simple and computationally inexpensive sensorial information can allow a mobile robot to precisely locate itself. They also demonstrate the generality of the proposed fusion and integration mechanism.

The author proposes a general model for representing sensorial information and its uncertainty, the symmetries and perturbation model (SPmodel). In the model the intrinsic partiality of geometric information is represented in terms of symmetries of the involved geometric elements. Location uncertainty due to sensor imprecision is represented by means of a local perturbation, expressed in a reference frame attached to the geometric element, with an associated probabilistic model. Using the SPmodel, a method is developed for integrating geometric information that allows estimation of the location of a feature or an object from a set of partial and uncertain observations. The integration mechanisms are based on extended Kalman filter theory.>