Continuous Sensor Registration

We develop a fundamentally novel formulation of the sensor registration problem that is continuous and models the action of an arbitrary Lie group on any smooth manifold. Our solution uses the integration of the flow in the Lie algebra by maximizing the inner product between two functions defined over the fixed and moving measurements sets. The continuity is achieved through functional treatment of the problem and representing the functions in a reproducing kernel Hilbert space. We apply the developed framework to the particular, and commonly used, case of RGB-D images (depth camera) and SE(n) matrix Lie group. As a result, the registration is not limited to the specific image resolution. As opposed to the current direct energy formulation, which involves computation of the numerical image intensity gradient to be used in conjunction with the analytical Jacobian of the pose via the chain rule, our framework is fully analytical and the gradient has a complete closed-form derivation. By being feature-free, continuous visual odometry improves tracking ability when the environment lacks evident structure or texture. Code:

Related Publications

Robotic Exploration, Information Gathering, and Environmental Monitoring

The Incrementally-exploring Information Gathering (IIG) algorithms are sampling-based motion planning algorithms equipped with an information-theoretic convergence criterion for incremental informative motion planning. The proposed approach allows for dense map representation and incorporates the full state uncertainty into the planning process. The problem is formulated as a maximization problem with a budget constraint. Our approach is built on Rapidly-exploring Information Gathering (RIG) algorithms and benefits from the advantages of sampling-based optimal motion planning algorithms. Code:

A MATLAB implementation of the incremental Gaussian processes occupancy mapping using range-finder sensors is available here:

Related Publications

Legged Robot State Estimation

We developed a state estimator, i.e, Right Invariant Extended Kalman Filter (RI-EKF) for legged robots that is robust to initialization and disturbance errors. As a result, it is suitable for robots that work in rough and unstructured environments. In particular, legged robots that are likely to engage in dynamic locomotion, running, and aggressive maneuvers benefit from this state estimator. Code:


We also developed a long-term state estimation and mapping framework for legged robots that allows for reducing the drift and correcting the past estimates as the robot perceives new information. During long-term missions, odometry systems can drift substantially, leading to an unbounded growth in the covariance of the estimate and an undesirable expansion of the search space for data association tasks. We used the factor graph smoothing framework for building such systems in which real-time performance is achieved by exploiting the sparse structure of the Simultaneous Localization and Mapping (SLAM) problem.

Related Publications

Semantic Robotic Perception

Building on geometric-based techniques, semantic perception of the environment, such as outdoor terrain classes and indoor object categories can enable safe and efficient navigation as well as scene understanding in such environments. However, the incorporation of semantic knowledge into the map of the robot’s surrounding makes the map inference problem theoretically and computationally much more challenging. We tackle the problem of semantic sensor registration and real-time dense semantic robotic mapping for both outdoor and indoor environments to enable semantically-aware autonomous navigation. 

The software for the work on Semantic ICP which is published in BMVC is available as an open source implementation at

Related Publications

Radio Signal-based Localization and Tracking

Wireless Local Area Network (WLAN) and Bluetooth Low Energy (BLE) technologies are widespread and ubiquitous. Radio signals can be a complementary source of information alongside widely used sensors such as cameras. However, using cameras can raise privacy concerns, depending on the deployed location, which can limit the application of such systems. On the other hand, in indoors, radio signals are severely impacted due to shadowing and multipathing effects which make the available wireless-based positioning systems less accurate (1-10 m). We propose a radio-inertial localization and tracking system that exploits BLE, Inertial Measurement Unit (IMU), and magnetometer sensors with the quality available in standard smartphones.

Related Publications