Todor Stoyanov

Fast and accurate scan registration through minimization of the distance between compact 3D NDT representations

Registration of range sensor measurements is an important task in mobile robotics and has received a lot of attention. Several iterative optimization schemes have been proposed in order to align three-dimensional (3D) point scans. With the more widespread use of high-frame-rate 3D sensors and increasingly more challenging application scenarios for mobile robots, there is a need for fast and accurate registration methods that current state-of-the-art algorithms cannot always meet. This work proposes a novel algorithm that achieves accurate point cloud registration an order of a magnitude faster than the current state of the art. The speedup is achieved through the use of a compact spatial representation: the Three-Dimensional Normal Distributions Transform (3D-NDT). In addition, a fast, global-descriptor based on the 3D-NDT is defined and used to achieve reliable initial poses for the iterative algorithm. Finally, a closed-form expression for the covariance of the proposed method is also derived. The proposed algorithms are evaluated on two standard point cloud data sets, resulting in stable performance on a par with or better than the state of the art. The implementation is available as an open-source package for the Robot Operating System (ROS).

Path planning in 3D environments using the Normal Distributions Transform

Planning feasible paths in fully three-dimensional environments is a challenging problem. Application of existing algorithms typically requires the use of limited 3D representations that discard potentially useful information. This article proposes a novel approach to path planning that utilizes a full 3D representation directly: the Three-Dimensional Normal Distributions Transform (3D-NDT). The well known wavefront planner is modified to use 3D-NDT as a basis for map representation and evaluated using both indoor and outdoor data sets. The use of 3D-NDT for path planning is thus demonstrated to be a viable choice with good expressive capabilities.

Point set registration through minimization of the L 2 distance between 3D-NDT models

Point set registration-the task of finding the best fitting alignment between two sets of point samples, is an important problem in mobile robotics. This article proposes a novel registration algorithm, based on the distance between Three-Dimensional Normal Distributions Transforms. 3D-NDT models - a sub-class of Gaussian Mixture Models with uniformly weighted, largely disjoint components, can be quickly computed from range point data. The proposed algorithm constructs 3D-NDT representations of the input point sets and then formulates an objective function based on the L2 distance between the considered models. Analytic first and second order derivatives of the objective function are computed and used in a standard Newton method optimization scheme, to obtain the best-fitting transformation. The proposed algorithm is evaluated and shown to be more accurate and faster, compared to a state of the art implementation of the Iterative Closest Point and 3D-NDT Point-to-Distribution algorithms.

Normal Distributions Transform Occupancy Maps: Application to large-scale online 3D mapping

Autonomous vehicles operating in real-world industrial environments have to overcome numerous challenges, chief among which is the creation and maintenance of consistent 3D world models. This paper proposes to address the challenges of online real-world mapping by building upon previous work on compact spatial representation and formulating a novel 3D mapping approach - the Normal Distributions Transform Occupancy Map (NDT-OM). The presented algorithm enables accurate real-time 3D mapping in large-scale dynamic environments employing a recursive update strategy. In addition, the proposed approach can seamlessly provide maps at multiple resolutions allowing for fast utilization in high-level functions such as localization or path planning. Compared to previous approaches that use the NDT representation, the proposed NDT-OM formulates an exact and efficient recursive update formulation and models the full occupancy of the map.

SDF Tracker: A parallel algorithm for on-line pose estimation and scene reconstruction from depth images

Ego-motion estimation and environment mapping are two recurring problems in the field of robotics. In this work we propose a simple on-line method for tracking the pose of a depth camera in six degrees of freedom and simultaneously maintaining an updated 3D map, represented as a truncated signed distance function. The distance function representation implicitly encodes surfaces in 3D-space and is used directly to define a cost function for accurate registration of new data. The proposed algorithm is highly parallel and achieves good accuracy compared to state of the art methods. It is suitable for reconstructing single household items, workspace environments and small rooms at near real-time rates, making it practical for use on modern CPU hardware.

Normal distributions transform Monte-Carlo localization (NDT-MCL)

Industrial applications often impose hard requirements on the precision of autonomous vehicle systems. As a consequence industrial Automatically Guided Vehicle (AGV) systems still use high-cost infrastructure based positioning solutions. In this paper we propose a map based localization method that fulfills the requirements on precision and repeatability, typical for industrial application scenarios. The proposed method - Normal Distributions Transform Monte Carlo Localization (NDT-MCL) is based on a well established probabilistic framework. In a novel contribution, we formulate the MCL localization approach using the Normal Distributions Transform (NDT) as an underlying representation for both map and sensor data. By relaxing the hard discretization assumption imposed by grid-map models and utilizing the piece-wise continuous NDT representation the proposed algorithm achieves substantially improved accuracy and repeatability. The proposed NDT-MCL algorithm is evaluated using offline data sets from both a laboratory and a real-world industrial environments. Additionally, we report a comparison of the proposed algorithm to grid-based MCL and to a commercial localization system when used in a closed-loop with the control system of an AGV platform. In all tests the proposed algorithm is demonstrated to provide performance superior to that of standard grid-based MCL and comparable to the performance of the commercial infrastructure based positioning system.

Augmented autonomy: Improving human-robot team performance in Urban search and rescue

Exploration of unknown environments remains one of the fundamental problems of mobile robotics. It is also a prime example for a task that can benefit significantly from multi-robot teams. We present an integrated system for semi-autonomous cooperative exploration, augmented by an intuitive user interface for efficient human supervision and control. In this preliminary study we demonstrate the effectiveness of the system as a whole and the intuitive interface in particular. Congruent with previous findings, results confirm that having a human in the loop improves task performance, especially with larger numbers of robots. Specific to our interface, we find that even untrained operators can efficiently manage a decently sized team of robots.

3D normal distributions transform occupancy maps: An efficient representation for mapping in dynamic environments

In order to enable long-term operation of autonomous vehicles in industrial environments numerous challenges need to be addressed. A basic requirement for many applications is the creation and maintenance of consistent 3D world models. This article proposes a novel 3D spatial representation for online real-world mapping, building upon two known representations: normal distributions transform (NDT) maps and occupancy grid maps. The proposed normal distributions transform occupancy map (NDT-OM) combines the advantages of both representations; compactness of NDT maps and robustness of occupancy maps. One key contribution in this article is that we formulate an exact recursive updates for NDT-OMs. We show that the recursive update equations provide natural support for multi-resolution maps. Next, we describe a modification of the recursive update equations that allows adaptation in dynamic environments. As a second key contribution we introduce NDT-OMs and formulate the occupancy update equations that allow to build consistent maps in dynamic environments. The update of the occupancy values are based on an efficient probabilistic sensor model that is specially formulated for NDT-OMs. In several experiments with a total of 17 hours of data from a milk factory we demonstrate that NDT-OMs enable real-time performance in large-scale, long-term industrial setups.

Beyond points: Evaluating recent 3D scan-matching algorithms

Given that 3D scan matching is such a central part of the perception pipeline for robots, thorough and large-scale investigations of scan matching performance are still surprisingly few. A crucial part of the scientific method is to perform experiments that can be replicated by other researchers in order to compare different results. In light of this fact, this paper presents a thorough comparison of 3D scan registration algorithms using a recently published benchmark protocol which makes use of a publicly available challenging data set that covers a wide range of environments. In particular, we evaluate two types of recent 3D registration algorithms - one local and one global. Both approaches take local surface structure into account, rather than matching individual points. After well over 100 000 individual tests, we conclude that algorithms using the normal distributions transform (NDT) provides accurate results compared to a modern implementation of the iterative closest point (ICP) method, when faced with scan data that has little overlap and weak geometric structure. We also demonstrate that the minimally uncertain maximum consensus (MUMC) algorithm provides accurate results in structured environments without needing an initial guess, and that it provides useful measures to detect whether it has succeeded or not. We also propose two amendments to the experimental protocol, in order to provide more valuable results in future implementations.

On the accuracy of the 3D Normal Distributions Transform as a tool for spatial representation

The Three-Dimensional Normal Distributions Transform (3D-NDT) is a spatial modeling technique with applications in point set registration, scan similarity comparison, change detection and path planning. This work concentrates on evaluating three common variations of the 3D-NDT in terms of accuracy of representing sampled semi-structured environments. In a novel approach to spatial representation quality measurement, the 3D geometrical modeling task is formulated as a classification problem and its accuracy is evaluated with standard machine learning performance metrics. In this manner the accuracy of the 3D-NDT variations is shown to be comparable to, and in some cases to outperform that of the standard occupancy grid mapping model.