The problem of planning sensing trajectories for a mobile robot to collect observations of a target and predict its future trajectory is known as active target tracking. Enabled by probabilistic motion models, one may solve this problem by exploring the belief space of all trajectory predictions given future sensing actions to maximise information gain. However, for multi-modal motion models the notion of information gain is often ill-defined. This paper proposes a planning approach designed around maximising information regarding the target's homotopy class, or high-level motion. We introduce homotopic information gain, a measure of the expected high-level trajectory information given by a measurement. We show that homotopic information gain is a lower bound for metric or low-level information gain, and is as sparsely distributed in the environment as obstacles are. Planning sensing trajectories to maximise homotopic information results in highly accurate trajectory estimates with fewer measurements than a metric information approach, as supported by our empirical evaluation on real and simulated pedestrian data.

Human-Robot Collaboration (HRC) scenarios demand computationally efficient frameworks that enable natural and safe actions and interactions in shared workspaces. To address this, we propose a novel framework that utilises interactive Gaussian Process (GP) distance fields applying Riemannian Motion Policies (RMP) for key HRC functionality. Unlike traditional Euclidean distance field methods, our framework provides continuous and differentiable distance fields resulting in smooth collision avoidance, efficient updates in dynamic scenes and readily available surface information such as normal vectors and curvature. By leveraging RMPs, our framework supports fast, reactive motion generation, utilising both the distance and gradient fields generated by the GP model. In addition, we propose a Hessian-based normal vector estimation technique that elegantly leverages the GP's second-order derivative information which we utilise for object manipulation. We demonstrate the versatility of our CPU-only system in common HRC scenarios where a collaborative robot (cobot) interacts safely and naturally with a human and performs grasping actions in a dynamic environment. Our framework offers an open-source11https://uts-ri.github.io/IDMP-RMP/, comprehensive and low-computational resource solution for HRC, making it an ideal tool for conducting a wide range of user studies. By providing a continuous and differentiable distance field and combining motion generation, obstacle avoidance, and object manipulation within a single system, we aim to broaden the scope and accessibility of HRC research in real dynamic environments.

In-orbit automated servicing is a promising path towards lowering the cost of satellite operations and reducing the amount of orbital debris. For this purpose, we present a pipeline for automated satellite docking port detection and state estimation using monocular vision data from standard RGB sensing or an event camera. Rather than taking snapshots of the environment, an event camera has independent pixels that asynchronously respond to light changes, offering advantages such as high dynamic range, low power consumption and latency. This work focuses on satellite-agnostic operations (only a geometric knowledge of the actual port is required) using the recently released Lockheed Martin Mission Augmentation Port (LM-MAP) as the target. By leveraging shallow data-driven techniques to preprocess the incoming data to highlight the LM-MAP's reflective navigational aids and then using basic geometric models for state estimation, we present a lightweight and data-efficient pipeline that can be used independently with either RGB or event cameras. We demonstrate the soundness of the pipeline and perform a quantitative comparison of the two modalities based on data collected with a photometrically accurate test bench that includes a robotic arm to simulate the target satellite's uncontrolled motion. The data has been made publicly available: https://uts-ri.githubio/rgb_event_docking_port/.

This paper presents DynORecon, a Dynamic Object Reconstruction system that leverages the information provided by Dynamic SLAM to simultaneously generate a volumetric map of observed moving entities while estimating free space to support navigation. By capitalising on the motion estimations provided by Dynamic SLAM, DynORecon continuously refines the representation of dynamic objects to eliminate residual artefacts from past observations and incrementally reconstructs each object, seamlessly integrating new observations to capture previously unseen structures. Our system is highly efficient (~20 FPS) and produces accurate (~10 cm) object reconstructions using simulated and real-world outdoor datasets.

Map matching and registration are essential tasks in robotics for localisation and integration of multi-session or multi-robot data. Traditional methods rely on cameras or LiDARs to capture visual or geometric information but struggle in challenging conditions like smoke or dust. Magnetometers, on the other hand, detect magnetic fields, revealing features invisible to other sensors and remaining robust in such environments. In this paper, we introduce Mag-Match, a novel method for extracting and describing features in 3D magnetic vector field maps to register different maps of the same area. Our feature descriptor, based on higher-order derivatives of magnetic field maps, is invariant to global orientation, eliminating the need for gravity-aligned mapping. To obtain these higher-order derivatives map-wide given point-wise magnetometer data, we leverage a physics-informed Gaussian process to perform efficient and recursive probabilistic inference of both the magnetic field and its derivatives. We evaluate MagMatch in simulated and real-world experiments against a SIFT-based approach, demonstrating accurate map-to-map, robot-to-map, and robot-to-robot transformations—even without initial gravitational alignment.

Over the past decade, lidars have become a cornerstone of robotics state estimation and perception thanks to their ability to provide accurate geometric information about their surroundings in the form of 3D scans. Unfortunately, most of nowadays lidars do not take snapshots of the environment but sweep the environment over a period of time (typically around 100 ms). Such a rolling-shutter-like mechanism introduces motion distortion into the collected lidar scan, thus hindering downstream perception applications. In this paper, we present a novel method for motion distortion correction of lidar data by tightly coupling lidar with Inertial Measurement Unit (IMU) data. The motivation of this work is a map-free dynamic object detection based on lidar. The proposed lidar data undistortion method relies on continuous preintegrated of IMU measurements that allow parameterising the sensors’ continuous 6-DoF trajectory using solely eleven discrete state variables (biases, initial velocity, and gravity direction). The undistortion consists of feature-based distance minimisation of point-to-line and point-to-plane residuals in a non-linear least-square formulation. Given undistorted geometric data over a short temporal window, the proposed pipeline computes the spatiotemporal normal vector of each of the lidar points. The temporal component of the normals is a proxy for the corresponding point’s velocity, therefore allowing for learning-free dynamic object classification without the need for registration in a global reference frame. We demonstrate the soundness of the proposed method and its different components using public datasets and compare them with state-of-the-art lidar-inertial state estimation and dynamic object detection algorithms.

In this paper, we propose a robot skill-learning method that facilitates fast adaption to new tasks online. Our method is based on a hybrid learning from demonstration and reinforcement learning approach, which seeds learning with a compact and structured skill model, leading to efficient and stable behaviours. To facilitate fast skill adaption, we propose a bootstrapped learning framework that learns a policy for adapting a skill model across a wide range of initial conditions in simulation. This policy is then used to bootstrap a refinement process that quickly adapts the learnt skill model to new initial conditions in a few learning iterations. Our refined skill model is designed to be deployable on hardware and can correct for discrepancies between the simulation and the real world. Furthermore, we propose a novel method for constraining policy exploration to promising trajectories, which is crucial for enabling manipulation in complex environments. We evaluate our framework in simulation and hardware in multiple environments with varying task complexity. We showcase that compared to the state-of-the-art, which achieves an average success rate of only 56.6% across three different tasks of varying difficulty, our algorithm significantly outperforms it with an average success rate of 90%.

Abstract Garment alteration is a practical technique to adapt an existing garment to fit a target body shape. Typically executed by skilled tailors, this process involves a series of strategic fabric operations—removing or adding material—to achieve the desired fit on a target body. We propose an innovative approach to automate this process by computing a set of practically feasible modifications that adapt an existing garment to fit a different body shape. We first assess the garment's fit on a reference body; then, we replicate this fit on the target by deriving a set of pattern modifications via a linear program. We compute these alterations by employing an iterative process that alternates between global geometric optimization and physical simulation. Our method utilizes geometry‐based simulation of woven fabric's anisotropic behavior, accounts for tailoring details like seam matching, and incorporates elements such as darts or gussets. We validate our technique by producing digital and physical garments, demonstrating practical and achievable alterations.

Localisation is one of the key capabilities for autonomous robots with sensors. Magnetic sensors to perceive the environment, although less explored, are an alternative modality to aid localisation. This paper proposes the use of continuous vector fields provided by a Gaussian Process (GP) with a divergence-free kernel that follows the magnetic flux to localise a mobile robot moving in a 2D space. The environment is pre-mapped in 3D producing a magnetic vector field with a GP that can be queried at any location. By means of a particle filter for Monte Carlo localisation, the mobile robot can be globally localised in the environment. We validate our approach using simulations and experimental results. Complex simulated environments using ANSYS are exploited to show our approach outperforms commonly used kernels.

Keypoint annotation in pointclouds is an important task for 3D reconstruction, object tracking and alignment, in particular in deformable or moving scenes. In the context of agriculture robotics, it is a critical task for livestock automation to work toward condition assessment or behaviour recognition. In this work, we propose a novel approach for semantic keypoint annotation in pointclouds, by reformulating the keypoint extraction as a regression problem of the distance between the keypoints and the rest of the pointcloud. We use the distance on the pointcloud manifold mapped into a radial basis function (RBF), which is then learned using an encoder-decoder architecture. Special consideration is given to the data augmentation specific to multi-depth-camera systems by considering noise over the extrinsic calibration and camera frame dropout. Additionally, we investigate computationally efficient non-rigid deformation methods that can be applied to animal pointclouds. Our method is tested on data collected in the field, on moving beef cattle, with a calibrated system of multiple hardware-synchronised RGB-D cameras.

This work addresses the issue of motion compensation and pattern tracking in event camera data. An event camera generates asynchronous streams of events triggered independently by each of the pixels upon changes in the observed intensity. Providing great advantages in low-light and rapid-motion scenarios, such unconventional data present significant research challenges as traditional vision algorithms are not directly applicable to this sensing modality. The proposed method decomposes the tracking problem into a local SE(2) motion-compensation step followed by a homography registration of small motion-compensated event batches. The first component relies on Gaussian Process (GP) theory to model the continuous occupancy field of the events in the image plane and embed the camera trajectory in the covariance kernel function. In doing so, estimating the trajectory is done similarly to GP hyperparameter learning by maximising the log marginal likelihood of the data. The continuous occupancy fields are turned into distance fields and used as templates for homography-based registration. By benchmarking the proposed method against other state-of-the-art techniques, we show that our open-source implementation performs high-accuracy motion compensation and produces high-quality tracks in real-world scenarios.

Learning from demonstration (LfD) has the potential to greatly increase the applicability of robotic manipulators in modern industrial applications. Recent progress in LfD methods have put more emphasis in learning robustness than in guiding the demonstration itself in order to improve robustness. The latter is particularly important to consider when the target system reproducing the motion is structurally different to the demonstration system, as some demonstrated motions may not be reproducible. In light of this, this paper introduces a new guided learning from demonstration paradigm where an interactive graphical user interface (GUI) guides the user during demonstration, preventing them from demonstrating non-reproducible motions. The key aspect of our approach is determining the space of reproducible motions based on a motion planning framework which finds regions in the task space where trajectories are guaranteed to be of bounded length. We evaluate our method on two different setups with a six-degree-of-freedom (DOF) UR5 as the target system. First our method is validated using a seven-DOF Sawyer as the demonstration system. Then an extensive user study is carried out where several participants are asked to demonstrate, with and without guidance, a mock weld task using a hand held tool tracked by a VICON system. With guidance users were able to always carry out the task successfully in comparison to only 44% of the time without guidance.

This paper presents an active visual-inertial mapping framework with points and planes. The key aspect of the proposed framework is a novel probabilistic plane extraction with its associated model for estimation. The approach allows the extraction of plane parameters and their uncertainties based on a modified version of PlaneRCNN [1]. The extracted probabilistic plane features are fused with point features in order to increase the robustness of the estimation system in texture-less environments, where algorithms based on points alone would struggle. A visual-inertial framework based on Iterative Extended Kalman filter (IEKF) is used to demonstrate the approach. The IEKF equations are customized through a measurement extrapolation method, which enables the estimation to handle the delay introduced by the neural network inference time systematically. The system is encompassed within an active mapping framework, based on Informative Path Planning to find the most informative path for minimizing map uncertainty in visual-inertial systems. The results from the conducted experiments with a stereo/IMU system mounted on a robotic arm show that introducing planar features to the map, in order to complement the point features in the state estimation, improves robustness in texture-less environments.

Trajectory prediction in a cluttered environment is key to many important robotics tasks such as autonomous navigation. However, there are an infinite number of possible trajectories to consider. To simplify the space of trajectories under consideration, we utilise homotopy classes to partition the space into countably many mathematically equivalent classes. All members within a class demonstrate identical high-level motion with respect to the environment, i.e., travelling above or below an obstacle. This allows high-level prediction of a trajectory in terms of a sparse label identifying its homotopy class. We therefore present a light-weight learning framework based on variable-order Markov processes to learn and predict homotopy classes and thus high-level agent motion. By informing a Gaussian mixture model (GMM) with our homotopy class predictions, we see great improvements in low-level trajectory prediction compared to a naive GMM on a real dataset.

Robots reason about the environment through dedicated representations. Despite the fact that Gaussian Process (GP)-based representations are appealing due to their probabilistic and continuous nature, the cubic computational complexity is a concern. In this paper, we present a novel efficient GP-based representation that has the ability to produce accurate distance fields and is parameterised by the optimal locations of pseudo inputs. When applying the proposed method together with a kernel approximation approach, we show it outperforms well-established sparse GP frameworks in efficiency and accuracy. Moreover, we extend the proposed method to work in a dynamic setting, where a map is built iteratively and the scene dynamics are accounted for by adding or removing objects from the environment representation. In a nutshell, our method provides the ability to infer dynamic distance fields and achieve state-of-the-art reconstruction efficiently.

Whereas dedicated scene representations are required for each different task in conventional robotic systems, this article demonstrates that a unified representation can be used directly for multiple key tasks. We propose the log-Gaussian process implicit surface for mapping, odometry, and planning (Log-GPIS-MOP): a probabilistic framework for surface reconstruction, localization, and navigation based on a unified representation. Our framework applies a logarithmic transformation to a Gaussian process implicit surface (GPIS) formulation to recover a global representation that accurately captures the Euclidean distance field with gradients and, at the same time, the implicit surface. By directly estimating the distance field and its gradient through Log-GPIS inference, the proposed incremental odometry technique computes the optimal alignment of an incoming frame and fuses it globally to produce a map. Concurrently, an optimization-based planner computes a safe collision-free path using the same Log-GPIS surface representation. We validate the proposed framework on simulated and real datasets in 2-D and 3-D, and benchmark against the state-of-the-art approaches. Our experiments show that Log-GPIS-MOP produces competitive results in sequential odometry, surface mapping, and obstacle avoidance.

We present a planning framework for min-imising the deterministic worst-case error in sparse Gaus-sian process (GP) regression. We first derive a univer-sal worst-case error bound for sparse GP regression with bounded noise using interpolation theory on reproducing kernel Hilbert spaces (RKHSs). By exploiting the conditional inde-pendence (CI) assumption central to sparse GP regression, we show that the worst-case error minimisation can be achieved by solving a posterior entropy minimisation problem. In turn, the posterior entropy minimisation problem is solved using a Gaussian belief space planning algorithm. We corroborate the proposed worst-case error bound in a simple 1D example, and test the planning framework in simulation for a 2D vehicle in a complex flow field. Our results demonstrate that the proposed posterior entropy minimisation approach is effective in minimising deterministic error, and outperforms the conventional measurement entropy maximisation formulation when the inducing points are fixed.

In this paper, we introduce an event-based visual odometry and mapping framework that relies on decaying event-based corners. Event cameras, unlike conventional cam-eras, can provide sensor data during high-speed motions or in scenes with high dynamic ranges. Rather than providing intensity information at a global shutter rate, events are trig-gered asynchronously depending on whether there is a change in brightness at the pixel location. This novel sensing paradigm calls for unconventional ego-motion estimation techniques to address these new challenges. The key aspect of our framework is the use of a continuous representation of inertial measurements to characterise the system's motion which accommodates the asynchronous nature of the event data while estimating a discrete state in an optimisation-based approach. The proposed method relies on corners extracted from events-only data and associates them with a spatio-temporal locality scheme based on exponential decay. Event tracks are then tightly coupled with temporally accurate preintegrated inertial measurements, allowing for the estimation of ego-motion and a sparse map. The proposed method is evaluated on the Event Camera Dataset showing performance against the state-of-art in event-based visual-inertial odometry.

We propose a method for computing a sewing pattern of a given 3D garment model. Our algorithm segments an input 3D garment shape into patches and computes their 2D parameterization, resulting in pattern pieces that can be cut out of fabric and sewn together to manufacture the garment. Unlike the general state-of-the-art approaches for surface cutting and flattening, our method explicitly targets garment fabrication. It accounts for the unique properties and constraints of tailoring, such as seam symmetry, the usage of darts, fabric grain alignment, and a flattening distortion measure that models woven fabric deformation, respecting its anisotropic behavior. We bootstrap a recent patch layout approach developed for quadrilateral remeshing and adapt it to the purpose of computational pattern making, ensuring that the deformation of each pattern piece stays within prescribed bounds of cloth stress. While our algorithm can automatically produce the sewing patterns, it is fast enough to admit user input to creatively iterate on the pattern design. Our method can take several target poses of the 3D garment into account and integrate them into the sewing pattern design. We demonstrate results on both skintight and loose garments, showcasing the versatile application possibilities of our approach.

In this paper, we present a novel approach that predicts spatially and temporally crowd behaviour for robotic social navigation. Integrating mobile robots into human society involves the fundamental problem of navigation in crowds. A robot should attempt to navigate in a way that is minimally invasive to the humans in its environment. However, planning in a dynamic environment is difficult as the environment must be predicted into the future. This problem has been thoroughly studied considering the behaviour of pedestrians at the level of individuals. Instead, we represent a pedestrian crowd by its macroscopic properties over space, such as density and velocity. With this spatial representation, we propose to learn a convolutional recurrent model to predict these properties into the future. The key design of a probabilistic loss function capturing the crowd's macroscopic properties empowers the spatio-temporal crowd prediction. Using a social invasiveness metric defined on these properties predicted by our convolutional recurrent model, we develop a framework that produces globally-optimal plans in expectation. Extensive results using a realistic pedestrian simulator show the validity and performance of the proposed social navigation approach.

The state-of-the-art modern pose-graph optimization (PGO) systems are vertex based. In this context, the number of variables might be high, albeit the number of cycles in the graph (loop closures) is relatively low. For sparse problems particularly, the cycle space has a significantly smaller dimension than the number of vertices. By exploiting this observation, in this article, we propose an alternative solution to PGO that directly exploits the cycle space. We characterize the topology of the graph as a cycle matrix, and reparameterize the problem using relative poses, which are further constrained by a cycle basis of the graph. We show that by using a minimum cycle basis, the cycle-based approach has superior convergence properties against its vertex-based counterpart, in terms of convergence speed and convergence to the global minimum. For sparse graphs, our cycle-based approach is also more time efficient than the vertex-based. As an additional contribution of this work, we present an effective algorithm to compute the minimum cycle basis. Albeit known in computer science, we believe that this algorithm is not familiar to the robotics community. All the claims are validated by experiments on both standard benchmarks and simulated datasets. To foster the reproduction of the results, we provide a complete open-source C++ implementation1of our approach.

In this article, we present inertial lidar localization autocalibration and mapping: an offline probabilistic framework for localization, mapping, and extrinsic calibration based on a 3-D lidar and a six-degree-of-freedom inertial measurement unit. Most of today's lidars collect geometric information about the surrounding environment by sweeping lasers across their field of view. Consequently, 3-D points in one lidar scan are acquired at different timestamps. If the sensor trajectory is not accurately known, the scans are affected by the phenomenon known as motion distortion. The proposed method leverages preintegration with a continuous representation of the inertial measurements to characterize the system's motion at any point in time. It enables precise correction of the motion distortion without relying on any explicit motion model. The system's pose, velocity, biases, and time shift are estimated via a full batch optimization that includes automatically generated loop closure constraints. The autocalibration and the registration of lidar data rely on planar and edge features matched across pairs of scans. The performance of the framework is validated through simulated and real-data experiments.

Information gathering algorithms play a key role in unlocking the potential of robots for efficient data collection in a wide range of applications. However, most existing strategies neglect the fundamental problem of the robot pose uncertainty, which is an implicit requirement for creating robust, high-quality maps. To address this issue, we introduce an informative planning framework for active mapping that explicitly accounts for the pose uncertainty in both the mapping and planning tasks. Our strategy exploits a Gaussian Process (GP) model to capture a target environmental field given the uncertainty on its inputs. For planning, we formulate a new utility function that couples the localization and field mapping objectives in GP-based mapping scenarios in a principled way, without relying on manually-tuned parameters. Extensive simulations show that our approach outperforms existing strategies, reducing mean pose uncertainty and map error. We present a proof of concept in an indoor temperature mapping scenario.

In this paper, we introduce IDOL, an optimization-based framework for IMU-DVS Odometry using Lines. Event cameras, also called Dynamic Vision Sensors (DVSs), generate highly asynchronous streams of events triggered upon illumination changes for each individual pixel. This novel paradigm presents advantages in low illumination conditions and high-speed motions. Nonetheless, this unconventional sensing modality brings new challenges to perform scene reconstruction or motion estimation. The proposed method offers to leverage a continuous-time representation of the inertial readings to associate each event with timely accurate inertial data. The method's front-end extracts event clusters that belong to line segments in the environment whereas the back-end estimates the system's trajectory alongside the lines' 3D position by minimizing point-to-line distances between individual events and the lines' projection in the image space. A novel attraction/repulsion mechanism is presented to accurately estimate the lines' extremities, avoiding their explicit detection in the event data. The proposed method is benchmarked against a state-of-the-art frame-based visual-inertial odometry framework using public datasets. The results show that IDOL performs at the same order of magnitude on most datasets and even shows better orientation estimates. These findings can have a great impact on new algorithms for DVS.

The ability to recognize previously mapped locations is an essential feature for autonomous systems. Unstructured planetary-like environments pose a major challenge to these systems due to the similarity of the terrain. As a result, the ambiguity of the visual appearance makes state-of-the-art visual place recognition approaches less effective than in urban or man-made environments. This paper presents a method to solve the loop closure problem using only spatial information. The key idea is to use a novel continuous and probabilistic representations of terrain elevation maps. Given 3D point clouds of the environment, the proposed approach exploits Gaussian Process (GP) regression with linear operators to generate continuous gradient maps of the terrain elevation information. Traditional image registration techniques are then used to search for potential matches. Loop closures are verified by leveraging both the spatial characteristic of the elevation maps (SE (2) registration) and the probabilistic nature of the GP representation. A submap-based localization and mapping framework is used to demonstrate the validity of the proposed approach. The performance of this pipeline is evaluated and benchmarked using real data from a rover that is equipped with a stereo camera and navigates in challenging, unstructured planetary-like environments in Morocco and on Mt. Etna.

Multi-modal estimation systems have the advantage of increased accuracy and robustness. To achieve accurate sensor fusion with these types of systems, a reliable extrinsic calibration between each sensor pair is critical. This paper presents a novel self-calibration framework for lidar-inertial systems. The key idea of this work is to use an informative path planner to find the admissible path that produces the most accurate calibration of such systems in an unknown environment within a given time budget. This is embedded into a simultaneous localization, mapping and calibration lidar-inertial system, which involves challenges in dealing with agile motions for excitation and large amount of data. Our approach has two stages: firstly, the environment is explored and mapped following a pre-defined path; secondly, the map is exploited to find a continuous and differentiable path that maximises the information gain within a sampling-based planner. We evaluate the proposed self-calibration method in a simulated environment and benchmark it with standard predefined paths to show its performance.

In this paper, we introduce a probabilistic framework for INertial Lidar Localisation And MApping (IN2LAMA). Most of today's lidars are based on spinning mechanisms that do not capture snapshots of the environment. As a result, movement of the sensor can occur while scanning. Without a good estimation of this motion, the resulting point clouds might be distorted. In the lidar mapping literature, a constant velocity motion model is commonly assumed. This is an approximation that does not necessarily always hold. The key idea of the proposed framework is to exploit preintegrated measurements over upsampled inertial data to handle motion distortion without the need for any explicit motion-model. It tightly integrates inertial and lidar data in a batch on-manifold optimisation formulation. Using temporally precise upsampled preintegrated measurement allows frame-to-frame planar and edge features association. Moreover, features are re-computed when the estimate of the state changes, consolidating front-end and back-end interaction. We validate the effectiveness of the approach through simulated and real data.

In this paper, we present a probabilistic framework to recover the extrinsic calibration parameters of a lidar-IMU sensing system. Unlike global-shutter cameras, lidars do not take single snapshots of the environment. Instead, lidars collect a succession of 3D-points generally grouped in scans. If these points are assumed to be expressed in a common frame, this becomes an issue when the sensor moves rapidly in the environment causing motion distortion. The fundamental idea of our proposed framework is to use preintegration over interpolated inertial measurements to characterise the motion distortion in each lidar scan. Moreover, by using a set of planes as a calibration target, the proposed method makes use of lidar point-to-plane distances to jointly calibrate and localise the system using on-manifold optimisation. The calibration does not rely on a predefined target as arbitrary planes are detected and modelled in the first lidar scan. Simulated and real data are used to show the effectiveness of the proposed method.

Detecting and tracking people is a key requirement in the development of robotic technologies intended to operate in human environments. In crowded environments such as train stations this task is particularly challenging due the high numbers of targets and frequent occlusions. In this paper we present a framework for detecting and tracking humans in such crowded environments in terms of 2D pose ( x, y, θ). The main contributions are a method for extracting pose from the most visible parts of the body in a crowd, the head and shoulders, and a tracker which leverages social constraints regarding peoples orientation, movement and proximity to one another, to improve robustness in this challenging environment. The framework is evaluated on two datasets: one captured in a lab environment with ground truth obtained using a motion capture system, and the other captured in a busy inner city train station. Pose errors are reported against the ground truth and the tracking results are then compared with a state-of-the-art person tracking framework.

Robust online incremental SLAM applications require metrics to evaluate the impact of current measurements. Despite its prevalence in graph pruning, information-theoretic metrics solely are insufficient to detect outliers. The optimal value of the objective function is a better choice to detect outliers but cannot be computed unless the problem is solved. In this paper, we show how the objective function change can be predicted in an incremental pose-graph optimization scheme, without actually solving the problem. The predicted objective function change can be used to guide online decisions or detect outliers. Experiments validate the accuracy of the predicted objective function, and an application to outlier detection is also provided, showing its advantages over M-estimators.

In this paper, we present a method for real-time 3D sound sources mapping using an off-the-shelf robotic perception sensor equipped with a linear microphone array. Conventional approaches to map sound sources in 3D scenarios use dedicated 3D microphone arrays, as this type of arrays provide two degrees of freedom (DOF) observations. Our method addresses the problem of 3D sound sources mapping using a linear microphone array, which only provides one DOF observations making the estimation of the sound sources location more challenging. In the proposed method, multi hypotheses tracking is combined with a new sound source parametrisation to provide with a good initial guess for an online optimisation strategy. A joint optimisation is carried out to estimate 6 DOF sensor poses and 3 DOF landmarks together with the sound sources locations. Additionally, a dedicated sensor model is proposed to accurately model the noise of the Direction of Arrival (DOA) observation when using a linear microphone array. Comprehensive simulation and experimental results show the effectiveness of the proposed method. In addition, a real-time implementation of our method has been made available as open source software for the benefit of the community.

Building large-scale 2.5D maps when spatial correlations are considered can be quite expensive, but there are clear advantages when fusing data. While optimal submapping strategies have been explored previously in covariance-form using Gaussian Process for large-scale mapping, this paper focuses on transferring such concepts into information form. By exploiting the conditional independence property of the Gaussian Markov Random Field (GMRF) models, we propose a submapping approach to build a nearly optimal global 2.5D map. In the proposed approach data is fused by first fitting a GMRF to one sensor dataset; then conditional independent submaps are inferred using this model and updated individually with new data arrives. Finally, the information is propagated from submap to submap to later recover the fully updated map. This is efficiently achieved by exploiting the inherent structure of the GMRF, fusion and propagation all in information form. The key contribution of this paper is the derivation of the algorithm to optimally propagate information through submaps by only updating the common parts between submaps. Our results show the proposed method reduces the computational complexity of the full mapping process while maintaining the accuracy. The performance is evaluated on synthetic data from the Canadian Digital Elevation Data.

Unmanned aerial vehicles (UAVs) can offer timely and cost-effective delivery of high-quality sensing data. However, deciding when and where to take measurements in complex environments remains an open challenge. To address this issue, we introduce a new multiresolution mapping approach for informative path planning in terrain monitoring using UAVs. Our strategy exploits the spatial correlation encoded in a Gaussian Process model as a prior for Bayesian data fusion with probabilistic sensors. This allows us to incorporate altitude-dependent sensor models for aerial imaging and perform constant-time measurement updates. The resulting maps are used to plan information-rich trajectories in continuous 3-D space through a combination of grid search and evolutionary optimization. We evaluate our framework on the application of agricultural biomass monitoring. Extensive simulations show that our planner performs better than existing methods, with mean error reductions of up to 45% compared to traditional “lawnmower” coverage. We demonstrate proof of concept using a multirotor to map color in different environments.

© 2016 IEEE. The simultaneous localization and mapping (SLAM) problem has been a research focus for many years and have reached a mature state. However, more robust solutions to the SLAM problem are still required, especially in large noise level scenarios. Because of the strong non-linearity of the SLAM problem, it is vital to start from a good initial value to avoid being trapped in local minima. In this paper, we propose a new SLAM formulation transforming the unconstrained Least Squares formulation into a constrained optimization problem. Algorithms based on this new formulation can naturally start from good initial value. Different from other constrained optimization problem, this new formulation can be efficiently solved with Sequential Quadratic Programming (SQP) methods. Based on SQP, we propose an incremental SQP algorithm to solve SLAM, which shows great advantage over Gauss Newton (g2o implementation) when working in large noise level scenarios. Experimental results show the validity of the proposed approach.

In this paper, we tackle the direct and inverse problems for the Remote-Field Eddy-Current (RFEC) technology. The direct problem is the sensor model, where given the geometry the measurements are obtained. Conversely, the inverse problem is where the geometry needs to be estimated given the field measurements. These problems are particularly important in the field of Non-Destructive Testing (NDT) because they allow assessing the quality of the structure monitored. We solve the direct problem in a parametric fashion using Least Absolute Shrinkage and Selection Operation (LASSO). The proposed inverse model uses the parameters from the direct model to recover the thickness using least squares producing the optimal solution given the direct model. This study is restricted to the 2D axisymmetric scenario. Both, direct and inverse models, are validated using a Finite Element Analysis (FEA) environment with realistic pipe profiles.

2.5D maps are preferable for representing the environment owing to their compactness. When noisy observations from multiple diverse sensors at different resolutions are available, the problem of 2.5D mapping turns to how to compound the information in an effective and efficient manner. This paper proposes a generic probabilistic framework for fusing efficiently multiple sources of sensor data to generate amendable, high-resolution 2.5D maps. The key idea is to exploit the sparse structure of the information matrix. Gaussian Markov Random Fields are employed to learn a prior map, which uses the conditional independence property between spatial location to obtain a representation of the state with a sparse information matrix. This prior map encoded in information form can then be updated with other sources of sensor data in constant time. Later, mean state vector and variances can be also efficiently recovered using sparse matrices techniques. The proposed approach allows accurate estimation of 2.5D maps at arbitrary resolution, while incorporating sensor noise and spatial dependency in a statistically sound way. We apply the proposed framework to pipe wall thickness mapping and fuse data from two diverse sensors that have different resolutions. Experimental results are compared with three other methods, showing that, while greatly reducing computation time, the proposed framework is able to capture in large extend the spatial correlation to generate equivalent results to the computationally expensive optimal fusion method in covariance form with a Gaussian Process prior.

This work exploits modeling spatial correlation in 2.5D data using Gaussian Processes (GPs), and produces constrained sampling realizations on these models to improve certainty in the predictions by means of integrating additional sparse information. Data organized in 2.5D such as elevation and thickness maps has been extensively studied in the fields of robotics and geostatistics. These maps are typically represented as a probabilistic 2D grid that stores an estimated value (height or thickness) for each cell. With the increasing popularity and deployment of robotic devices for infrastructure inspection, 2.5D data becomes a common interpretation of the condition of the target being inspected. Modeling the spatial dependencies and making inferences on new grid locations is a common task that has been addressed using GPs, but inference results on locations which are weakly correlated with the training data are generally not sufficiently informative and distinctly uncertain. The predictive capability of the proposed framework, which is applicable to any 2.5D data, is demonstrated with field inspection data from pipelines. Specifically, sparse and complementary measurements from alternative sensing modalities have been incorporated into the model to predict in more detail local thickness conditions where GP training data is limited. The output of this work aims to probabilistically present variations of the target in the case that both accuracy and reasonable diversity are of significant interest.

In this paper, we propose a framework to map stationary sound sources while simultaneously localise a moving robot. Conventional methods for localisation and sound source mapping rely on a microphone array and either, 1) a proprioceptive sensor only (such as wheel odometry) or 2) an additional exteroceptive sensor (such as cameras or lasers) to get accurately the robot locations. Since odometry drifts over time and sound observations are bearing-only, sparse and extremely noisy, the former can only deal with relatively short trajectories before the whole map drifts. In comparison, the latter can get more accurate trajectory estimation over long distances and a better estimation of the sound source map as a result. However, in most of the work in the literature, trajectory estimation and sound source mapping are treated as uncorrelated, which means an update on the robot trajectory does not propagate properly to the sound source map. In this paper, we proposed an efficient method to correlate robot trajectory with sound source mapping by exploiting the conditional independence property between two maps estimated by two different Simultaneous Localisation and Mapping (SLAM) algorithms running in parallel. In our approach, the first map has the flexibility that can be built with any SLAM algorithm (filtering or optimisation) to estimate robot poses with an exteroceptive sensor. The second map is built by using a filtering-based SLAM algorithm locating all stationary sound sources parametrised with Inverse Depth Parametrisation (IDP). Robot locations used during IDP initialisation are the common features shared between the two SLAM maps, which allow to propagate information accordingly. Comprehensive simulations and experimental results show the effectiveness of the proposed method.

Typically 2.5D maps provide a compact and efficient representation of the environment. When sensor data is obtained from multiple sets of noisy measurements at differing resolutions, the problem of compounding this information together to provide an effective and efficient means of mapping is not trivial, particularly as the size of the environment increases. In this paper, we propose a general framework for integrating heterogeneous sensor data to obtain large-scale 2.5D probabilistic maps. Gaussian Processes are used to generate a prior map that learns the spatial correlation between nearby points. Bayesian data fusion is then employed to update these prior maps with new measurements from distinct sensor modalities. In order to deal with large scale data, a novel submapping strategy is introduced to perform the fusion step efficiently in dealing with large covariance matrices. Submaps are first marginalised from the learned correlated prior and then updated based on the property of conditional independence. Most notably, the technique lends itself to generate accurate estimates at arbitrary resolutions and is able to handle varying noise from disparate sensor sources. The framework is applied to pipeline thickness mapping, with experimental results in fusing a high-resolution sensor and a low-resolution sensor showing the ability of the proposed technique to capture spatial correlations to come up with more accurate results when compared with a naïve fusion approach.

An algorithm is proposed for matching data from different sensing modalities. The problem is formalised as a kidnapped robot problem, where Bayesian fusion is used to find the most likely location where both modalities agree. The key idea of our algorithm is to model the correlation between the two modalities as a likelihood used to update a location prior. Data, in this case, is represented as 2.5D thickness maps from a laser scanner and a Remote Field Eddy Current (RFEC) tool, used in non-destructive testing to assess the condition of infrastructures. The laser data is limited, while RFEC data is continuous. Given some prior in location, the aim is to find the 2.5D thickness map from the laser that corresponds to the RFEC data, which should be noted is highly noisy. Real data from CCTV inspections of water pipes are used to validate the proposed approach.

In this paper, an approach for sound source localisation and calibration of an asynchronous microphone array is proposed to be solved simultaneously. A graph-based Simultaneous Localisation and Mapping (SLAM) method is used for this purpose. Traditional sound source localisation using a microphone array has two main requirements. Firstly, geometrical information of microphone array is needed. Secondly, a multichannel analog-to-digital converter is required to obtain synchronous readings of the audio signal. Recent works aim at releasing these two requirements by estimating the time offset between each pair of microphones. However, it was assumed that the clock timing in each microphone sound card is exactly the same, which requires the clocks in the sound cards to be identically manufactured. A methodology is hereby proposed to calibrate an asynchronous microphone array using a graph-based optimisation method borrowed from the SLAM literature, effectively estimating the array geometry, time offset and clock difference/drift rate of each microphone together with the sound source locations. Simulation and experimental results are presented, which prove the effectiveness of the proposed methodology in achieving accurate estimates of the microphone array characteristics needed to be used on realistic settings with asynchronous sound devices.

Pipe thickness maps are used to assess the condition in pipelines. Thickness maps are a 2.5D representation similar to elevation maps in robotics. Probabilistic frameworks, however, have barely been used in this context. This paper presents a general approach for generating probabilistic maps from heterogeneous sensor data. The key idea is to learn the spatial correlation of a sensor through Gaussian Process models and use it as priors for Bayesian fusion. This approach is applied to the novel application of pipe thickness mapping. Data from a 3D laser scanner on the outer surface of the pipe and thickness measurements from a contact ultrasonic sensor are fused into a single thickness map with associated uncertainty. Moreover, a dedicated algorithm to model the ultrasonic sensor using kernel density estimation is also proposed. The overall approach is evaluated using the full 3D profile (outer and inner surfaces) of the pipe section as ground truth.

This article proposes a probabilistic approach to account for robot stability uncertainty when planing motions over uneven terrains. A novel probabilistic stability criterion derived from the cumulative distribution of a tip-over metric is introduced that allows a safety constraint to be dynamically updated by available sensor data as it becomes available. The proposed safety constraint authorizes the planner to generates more conservative motion plans for areas with higher levels of uncertainty, while avoids unnecessary caution in well-known areas. The proposed systematic approach is particularly applicable to reconfigurable robots that can assume safer postures when required, although is equally valid for fixed-configuration platforms to choose safer paths to follow. The advantages of planning with the proposed probabilistic stability metric are demonstrated with data collected from an indoor rescue arena, as well as an outdoor rover testing facility.

Assessment of the condition of underground pipelines is crucial to avoid breakages. Autonomous in-line inspection tools provided with Non-destructive Technology (NDT) sensors to assess large sections of the pipeline are commonly used for these purposes. An example of such sensors based on Eddy currents is the Remote Field Technology (RFT). A crucial step during in-line inspections is the detection of construction features, such as joints and elbows, to accurately locate and size specific defects within pipe sections. This step is often performed manually with the aid of visual data, which results in slow data processing. In this paper, we propose a generic framework to automate the detection and verification of these construction features using both NDT sensor data and visual images. Firstly, supervised learning is used to identify the construction features in the NDT sensor signals. Then, image processing is employed to verify the selection. Results are presented with data from a RFT tool, for which a specialised descriptor has been designed to characterise and classify its signal features. Furthermore, the construction feature is displayed in the image, once it is identified in the RFT data and detected in the visual data. A visual odometry algorithm has been implemented to locate the visual data with respect to the RFT data. About 800 meters of these multi-modal data are evaluated to test the validity of the proposed approach.

Early fire detection is crucial to minimise damage and save lives. Video surveillance smoke detectors do not suffer from transport delays and can cover large areas. The smoke detection on images is, however, a difficult problem due the variability of smoke density, lighting conditions, background clutter, and unstable patterns. In order to solve this problem, we propose a novel unsupervised object classifier. Single visual features are classified using a model that simultaneously creates a codebook and categorises the smoke using a bag-of-words paradigm based on LDA model. Our algorithm can also tell the amount of smoke present on the image. Multiple image sequences from different cameras are used to show the viability of the proposed approach. Our experiments show that the model generalises well for different cameras, perspectives and scales.

This paper proposes an approach to obtain a localisation that is robust to smoke by exploiting multiple sensing modalities: visual and infrared (IR) cameras. This localisation is based on a state-of-the-art visual SLAM algorithm. First, we show that a reasonably accurate localisation can be obtained in the presence of smoke by using only an IR camera, a sensor that is hardly affected by smoke, contrary to a visual camera (operating in the visible spectrum). Second, we demonstrate that improved results can be obtained by combining the information from the two sensor modalities (visual and IR cameras). Third, we show that by detecting the impact of smoke on the visual images using a data quality metric, we can anticipate and mitigate the degradation in performance of the localisation by discarding the most affected data. The experimental validation presents multiple trajectories estimated by the various methods considered, all thoroughly compared to an accurate dGPS/INS reference.

One of the most important problems in robot localisation is the detection of previously visited places (loops). When a robot closes a loop, the association between observed features and present ones can be used to update its position. The computational cost involved in the association process makes exhaustive loop search intractable. Most of the current techniques use observations of the environment as their main features to produce loop hypotheses. In this paper, we investigate the feasibility of producing loop candidates from features of the robot trajectory. We propose a new method for selecting loop-closure candidates based on an alignment likelihood function, which measures similarity between trajectory sequences. The algorithm is validated with data gathered in the city with our experimental platform. Positive results show that the trajectory has, indeed, features that can be extracted and applied to robot localisation. The resulting loop hypotheses may be regarded, for example, as a initialisation step to aid current methods.

A method for evaluating, at video rate, the quality of actions for a single camera while mapping unknown indoor environments is presented. The strategy maximizes mutual information between measurements and states to help the camera avoid making ill-conditioned measurements that are appropriate to lack of depth in monocular vision systems. Our system prompts a user with the appropriate motion commands during 6-DOF visual simultaneous localization and mapping with a handheld camera. Additionally, the system has been ported to a mobile robotic platform, thus closing the control-estimation loop. To show the viability of the approach, simulations and experiments are presented for the unconstrained motion of a handheld camera and for the motion of a mobile robot with nonholonomic constraints. When combined with a path planner, the technique safely drives to a marked goal while, at the same time, producing an optimal estimated map.

This paper presents 6-DOF monocular EKF-SLAM with undelayed initialization using linear landmarks with extensible endpoints, based on the Plu¿cker parametrization. A careful analysis of the properties of the Plu¿cker coordinates, defined in the projective space P5, permits their direct usage for undelayed initialization. Immediately after detection of a segment in the image, a Plu¿cker line is incorporated in the map. A single Gaussian pdf includes inside its 2-sigma region all possible lines given the observed segment, from arbitrarily close up to the infinity range, and in any orientation. The lines converge to stable 3D configurations as the moving camera gathers observations from new viewpoints. The line's endpoints, maintained out of the map, are constantly retro-projected from the image onto the line's local reference frame. An extending-only policy is defined to update them. We validate the method via Monte Carlo simulations and with real imagery data.

A large-scale mapping approach is combined with multiple robots events to achieve cooperative mapping. The mapping approach used is based on hierarchical SLAM -global level and local maps-, which is generalized for the multi-robot case. In particular, the consequences of multi-robot loop closing events (common landmarks detection and relative pose measurement between robots) are analyzed and managed at a global level. We present simulation results for each of these events using aerial and ground robots, and experimental results obtained with ground robots.

We present a fast line segment tracker which does not require any knowledge about the motion of the camera nor the structure of the observed scene. It runs on 320 times 240 pixel images at 30 Hz. We adapted the RAPiD tracker with a new way of handling multiple line hypotheses to deal with the simple model of a single line segment. We discuss the difficulty of using a chi2-test as merging criterion and also present a new approach to overcome it. Furthermore, instead of making assumptions about the camera motion, a constant velocity motion model to predict the line segment position in the following frame is used. We explain how to deal with the instability of the endpoint extraction in this motion model to avoid unintentional motion along the line. Finally, we present results on real world indoor and urban outdoor image sequences.

This paper explores the possibilities of using monocular simultaneous localization and mapping (SLAM) algorithms in systems with more than one camera. The idea is to combine in a single system the advantages of both monocular vision (bearings-only, infinite range observations but no 3-D instantaneous information) and stereovision (3-D information up to a limited range). Such a system should be able to instantaneously map nearby objects while still considering the bearing information provided by the observation of remote ones. We do this by considering each camera as an independent sensor rather than the entire set as a monolithic supersensor. The visual data are treated by monocular methods and fused by the SLAM filter. Several advantages naturally arise as interesting possibilities, such as the desynchronization of the firing of the sensors, the use of several unequal cameras, self-calibration, and cooperative SLAM with several independently moving cameras. We validate the approach with two different applications: a stereovision SLAM system with automatic self-calibration of the rig's main extrinsic parameters and a cooperative SLAM system with two independent free-moving cameras in an outdoor setting.

In this paper we present an observability analysis for a mobile robot performing SLAM with a single monocular camera. The aim is to get a better understanding of the well known intuitive behavior of these systems, such as the need for triangulation to features from different positions in order to get accurate relative pose estimates. The characterisation of the unobservable directions is made using the nullspace basis of the stripped observability matrix. This allows us to identify which vehicle motions are required to maximise the number of observable states in the system, which in turn affects accuracy in the estimation process. The analysis is performed by modelling the system in the continuous time domain as piecewise constant. Simulation results using an extended information filter are shown to verify the results of the observability analysis.

In this paper we consider a single hand-held camera performing SLAM at video rate with generic 6DOF motion. The aim is to optimise both the localisation of the sensor and building of the feature map by computing the most appropriate control actions or movements. The actions belong to a discrete set (e.g. go forward, go left, go up, turn right, etc), and are chosen so as to maximise the mutual information gain between posterior states and measurements. Maximising the mutual information helps the camera avoid making ill-conditioned measurements appropriate to bearing-only SLAM. Moreover, orientation changes are determined by maximising the trace of the Fisher information matrix. In this way, we allow the camera to continue looking at those landmarks with large uncertainty, but from better-posed directions. Various position and gaze control strategies are first tested in a simulated environment, and then validated in a video-rate implementation. Given that our system is capable of producing motion commands for a real-time 6DOF visual SLAM, it could be used with any type of mobile platform, without the need of other sensors

In this article we propose an algorithm to reduce the effects caused by linearization in the typical EKF approach to SLAM. The technique consists in computing the vehicle prior using an Unscented Transformation. The UT allows a better nonlinear mean and variance estimation than the EKF. There is no need however in using the UT for the entire vehicle-map state, given the linearity in the map part of the model. By applying the UT only to the vehicle states we get more accurate covariance estimates. The a posteriori estimation is made using a fully observable EKF step, thus preserving the same computational complexity as the EKF with sequential innovation. Experiments over a standard SLAM data set show the behavior of the algorithm.

In this article, we show marginal stability in SLAM, guaranteeing convergence to a non-zero mean state error estimate bounded by a constant value. Moreover, marginal stability guarantees also convergence of the Riccati equation of the one-step ahead state error covariance to at least one psd steady state solution. In the search for real-time implementations of SLAM, covariance inflation methods produce a suboptimal filter that eventually may lead to the computation of an unbounded state error covariance. We provide tight constraints in the amount of decorrelation possible, to guarantee convergence of the state error covariance, and at the same time, a linear-time implementation of SLAM.