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/.

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.

Vision-based self-localization is a crucial technology for enabling autonomous robot navigation in GPS-deprived environments. However, standard frame cameras are subject to motion blur and suffer from a limited dynamic range. This research focuses on efficient feature tracking for self-localization by using event-based cameras. Such cameras do not provide regular snapshots of the environment but asynchronously collect events that correspond to a small delta of illumination in each pixel independently, thus addressing the issue of motion blur during fast motion and high dynamic range. Specifically, we propose a continuous real-time asynchronous event-based feature tracking pipeline, named RATE. This pipeline integrates (i) a corner detector node utilizing a time slice of the Surface of Active Events to initialize trackers continuously, along with (ii) a tracker node with a proposed "tracking manager", consisting of a grid-based distributor to reduce redundant trackers and to remove feature tracks of poor quality. Evaluations using public datasets reveal that our method maintains a stable number of tracked features, and performs real-time tracking efficiently while maintaining or even improving tracking accuracy compared to state-of-the-art event-only tracking methods. Our ROS implementation is released as open-source: https://github.com/mikihiroikura/RATE

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.

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.

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.

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.

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.

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.