During my stay at CMU I collaborated in the development of a tightly-couple, fixed-lag visual-inertial odometry (VIO) framework using information sparsification. To bound computational complexity, fixed-lag smoothers typically marginalize out variables, but consequently introduce a densely connected linear prior which significantly deteriorates accuracy and efficiency. Current state-of-the-art approaches account for the issue by selectively discarding measurements and marginalizing additional variables. However, such strategies are sub-optimal from an information-theoretic perspective. Instead, our approach performs a dense marginalization step and preserves the information content of the dense prior. Our method sparsifies the dense prior with a nonlinear factor graph by minimizing the information loss. The resulting factor graph maintains information sparsity, structural similarity, and non-linearity. Our work was validated with real-time drone tests and perform comparisons to current state-of-the-art fixed-lag VIO methods in the EuRoC visual-inertial dataset. The experimental results show that the proposed method achieves competitive and superior accuracy in almost all trials.

[Hsiung et al. IROS'18 -to appear-]

Motion planning for Heavy-duty Vehicles

In the context of the European-funded Cargo-ANTs project, at Halmstad University we worked on motion planning techniques for Self-driving vehicles. Specifically, we focused on a Volvo FH16 Truck for efficient and safe freight transportation in main ports and freight terminals.

Similarly, in the context of the Swedish project ANTWaY we also worked in motion planning for Self-driving Trucks, but with the focus in work yards applications. We did this in collaboration with Volvo Trucks, Kollmorgen, Chalmers University and Halmstad University.

[Cargo-ANTs Web] [David et al. IROS'17] [David et al. FSR'17]

Dual-timescale NDT-MCL Localization

Industrial environments are rarely static and often their configuration is continuously changing due to the material transfer flow. This is a major challenge for infrastructure free localization systems. In this work we addressed this challenge by introducing a localization approach that uses a dual-timescale approach. The proposed approach - Dual-Timescale Normal Distributions Transform Monte Carlo Localization (DTNDT- MCL) - is a particle filter based localization method, which simultaneously keeps track of the pose using an apriori known static map and a short-term map. The short-term map is continuously updated and uses Normal Distributions Transform Occupancy maps to maintain the current state of the environment. A key novelty of this approach is that it does not have to select an entire timescale map but rather uses the best timescale locally.

Paper: [icra14]

[Video1] Approach overview.

[Video2] Navigation with this approach.

The probabilistic belief networks that result from standard feature-based simultaneous localization and map building cannot be directly used to plan trajectories. The reason is that they produce a sparse graph of landmark estimates and their probabilistic relations, which is of little value to find collision free paths for navigation. In contrast, this work showed that Pose SLAM graphs can be directly used as belief roadmaps. We introduced a path planning approach that devises optimal navigation strategies by searching for the path in the pose graph with lowest accumulated robot pose uncertainty.

Papers: [TRO-13] [icra11]

Downloads: [Code] (An example with Intel dataset)

[video1] (Path execution)

[video2] (Its application using 3D maps)

6DOF Pose SLAM using 3D LIDAR

This work dealt with the problem of 3D map building in urban settings for service robots, using three dimensional laser range scans as the main data input. The proposed solution consisted on the probabilistic alignment of 3D point clouds employing a 3D Pose SLAM implementation. We also introduced a technique to process the 3D volumetric maps obtained with our SLAM implementation to derive traversability maps. The datasets were acquired with a custom built 3D range scanner integrated into a mobile robot platform. The experimental site was the Barcelona Robot Lab, located at the Campus Nord of the Technical University of Catalonia, part of the European-funded project URUS.

Paper: [iros09]

[video]

Active Pose SLAM

This work consisted of an active exploration strategy that complements Pose SLAM and optimal navigation in Pose SLAM. The method evaluates the utility of exploratory and place revisiting sequences and chooses the one that minimizes overall map and path entropies. An advantage of the proposed strategy with respect to competing approaches is that to evaluate information gain over the map, only a very coarse prior map estimate needs to be computed. Its coarseness is independent and does not jeopardize the Pose SLAM estimate.

Paper: [iros12]

Exploration with Gaussian Process-based Maps

This work consisted of an active exploration strategy using Gaussian Process-based Maps to explore. The approach conveniently handles sparse sensor measurements to build a continuous model of the environment that exploits structural dependencies without the need to resort to a fixed resolution grid map. A gradient field of occupancy probability distribution is regressed from sensor data as a Gaussian process providing frontier boundaries for further exploration. The resulting continuous global frontier surface completely describes unexplored regions and, inherently, provides an automatic stop criterion for a desired sensitivity.

Papers: [icra14] [RSS13 Workshop] [ACRA 2013]

Informative Loop Closure

This work consisted of a loop closing technique to find the most informative links for Pose SLAM using stereo vision. The technique computes relative pose constraints via a robust least squares minimization of 3D point correspondences, which are in turn obtained from the matching of SIFT features over candidate image pairs. Our proposed loop closure test checks both for closeness of means and for highly informative updates at the same time.

Paper: [iros07]