SkiMap: An Efficient Mapping Framework for Robot Navigation

TL;DR

SkiMap introduces a multi-level, efficient mapping framework for robot navigation that supports real-time updates and diverse map representations using a novel Tree of SkipLists data structure.

Contribution

The paper presents a new mapping framework with a Tree of SkipLists that improves time efficiency and supports real-time map updates for robot navigation.

Findings

Faster than Octree-based methods in time efficiency

Maintains similar memory footprint for large maps

Supports real-time erosion and re-integration of measurements

Abstract

We present a novel mapping framework for robot navigation which features a multi-level querying system capable to obtain rapidly representations as diverse as a 3D voxel grid, a 2.5D height map and a 2D occupancy grid. These are inherently embedded into a memory and time efficient core data structure organized as a Tree of SkipLists. Compared to the well-known Octree representation, our approach exhibits a better time efficiency, thanks to its simple and highly parallelizable computational structure, and a similar memory footprint when mapping large workspaces. Peculiarly within the realm of mapping for robot navigation, our framework supports realtime erosion and re-integration of measurements upon reception of optimized poses from the sensor tracker, so as to improve continuously the accuracy of the map.

Tables

Table 1. TABLE I: Analysis of SkipList depth: tests performed on Freiburg Campus dataset with a resolution of 0.05 ​ m 0.05 𝑚 0.05m . The Table reports the average computation time to integrate new sensor measurements ( ∼ similar-to \sim 180k points), the average time for a full visit of the map and the memory footprint of the map.

SkipList Depth	Integration Time	Visiting Time	Memory
4	56 ms	215 ms	432 MB
8	29 ms	269 ms	588 MB
16	29 ms	215 ms	900 MB
32	32 ms	259 ms	1524 MB
64	33 ms	258 ms	2743 MB

Table 2. TABLE II: Percentage of memory savings with respect to a full 3D grid.

Dataset	Type	Memory Saving wrt 3D Grid
		Resolutions
		0.05m	0.1m	0.2m
Freiburg Campus $292\times 167\times 28m^3$
	octree	98.75%	96.21%	90.61%
	skimap	98.52%	94.28%	83.33%
New Dataset College $250\times 161\times 33m^3$
	octree	99.74%	99.00%	96.76%
	skimap	99.77%	98.84%	95.30%
Freiburg Long Office $23\times 25\times 10m^3$
	octree	90.50%	84.71%	74.91%
	skimap	82.62%	71.30%	54.63%

Equations

$$M_{OG}=\frac{x\times y\times z}{r^3}\times 4$$

$$M_{OCT}=n_{leaf}\times B_{leaf}+n_{inner}\times B_{inner}$$

$$M_{MLS}=\frac{(x\times y)}{r^2}\times B_{tile}+n_{voxels}\times B_{voxel}$$

$$I_x=\lfloor \frac{x}{r}\rfloor ,I_y=\lfloor \frac{y}{r}\rfloor ,I_z=\lfloor \frac{z}{r}\rfloor$$

$$\pi (\Gamma )=\sum _{i=min}^{max}\pi (f(I_{x_i}))$$

$$C=\bigcup _{i=min}^{max}{C}_{x_i}\mid C_{x_i}:\{\lfloor \frac{x}{r}\rfloor =I_{x_i}\}$$

$$I_{r^{+}},I_{r^{-}}=I\pm \lfloor \frac{radius}{resolution}\rfloor$$

$$P^{\prime }(v)=\frac{P(v)W(v)+p_i(v)w_i(i)}{W(v)+w_i(v)},W^{\prime }(v)=W(v)+w_i(v)$$

$$P^{\prime }(v)=\frac{P(v)W(v)-p_i(v)w_i(i)}{W(v)-w_i(v)},W^{\prime }(v)=W(v)-w_i(v)$$

Full text

I INTRODUCTION

Key to autonomous robot navigation is the ability to attain a sufficiently rich perception of the environment, so to allow the robot to plan a path, localize itself and avoid obstacles. This kind of perception is realized through suitable sensors and algorithms. As for the former, laser rangefinders have traditionally been employed to capture a planar view of the surroundings, while visual sensors, and in particular RGB-D cameras, are becoming more and more widespread on account of their potential to model the environment in 3D. In the space of algorithms, many proposals concern full-fledged SLAM (Simultaneous Localization And Mapping) systems aimed at both building a map of the workspace and localizing the sensor (i.e. the robot) therein. Other works, differently, are focused on the mapping task and address issues such as memory efficiency, quite mandatory to enable navigation in large spaces, and time efficiency, which concerns creating on-line the representation required by the navigation system, such as a 2D occupancy grid to plan a path through the environment or a 3D reconstruction to avoid obstacles reliably while a robot moves around or a 2.5 (aka height) map to assess free space at the flight altitude of a MAV (Micro Aerial Vehicle). Along the latter research line, in this paper we focus on mapping and present a novel approach, dubbed SkiMap, which is particularly time efficient and flexible enough to support seamlessly different kinds of representations that may be delivered on-line according to the application requirements (Figure LABEL:fig:teaser).

Another favorable trait of our mapping framework is the ability to erode and fuse back measurements in real-time upon receiving optimized poses from the sensor localization module in order to improve the accuracy of the map. Indeed, many recent sensor localization algorithms based on visual data can perform pose optimization on-line, e.g. upon detection of a loop closure, which holds the potential to continuously improve the map as long as sensor measurements may be injected therein according to the new optimized poses rather than the old ones.

Our framework has been implemented as a ready-to-use ROS [1] package freely distributed for research and education purposes111https://github.com/m4nh/skimap_ros. The package can be configured either to achieve mapping in conjunction with any external sensor localization module or as a full-fledged SLAM system, Slamdunk [2] providing camera poses optimized on-line in the latter option.

II RELATED WORK

As described in [3], the classical mapping approach for robot navigation is the 2D occupancy grid. Accordingly, sensors measurements (typically from planar laser scanners) are fused into a 2D Grid wherein each tile (i.e. a square chunk of the space) contains an occupancy probability which can be interpreted as the likelihood that the tile belongs to an obstacle. Many robot navigation systems, often referred to as Grid-Based SLAM, rely on this 2D occupancy grid [4], which is available in ROS [1] and can be considered as a baseline for robot navigation.

Yet, planar sensing and related 2D mapping may not be reliable enough due to defective reconstruction of the environment, e.g. when dealing with a MAV (micro aerial vehicle) for indoor navigation, or, more generally with any Mobile Robot that cannot be modeled as a bi-dimensional agent or any robot that has more than 3 DOFs. A conceptually straightforward approach to pursue 3D mapping when deploying sensors, e,g. visual sensors, capable of delivering 3D measurements would consist in extending the occupancy map to a 3D Grid by cutting the 3D space into Voxels (i.e. small cubes) [5], each voxel storing the probability for an obstacle to be located therein. However, handling a 3D occupancy grid may easily become impractical when dealing with large workspaces due to the excessive memory footprint; for example, should the probability stored in each voxel be encoded as a float number (4 Bytes), the 3D occupancy grid would require as many bytes of memory as

[TABLE]

$x,y,z$ being the sizes of each of the dimensions of the workspace and $r$ the voxel resolution (i.e. the voxel size expressed in the same units as $x,y,z$).

A quite popular memory efficient alternative to the 3D occupancy grid is the Octree [6], whereby the 3D Space is recursively partitioned into octants (octants being equivalent of quadrants in the 3D space) until voxels take the desired resolution. As such, this data structure is a tree in which each node has exactly 8 children; unlike the 3D grid, though, the Octree avoids modeling the empty space as only leaf and inner nodes associated with occupied space need to be allocated, thereby yielding significant memory savings when representing large environments. Hence, well-known mapping frameworks like Octomap [7] rely on this kind of data structure to build the required workspace representation. In particular, Hornung et. al.[7] shape their data structure so that voxels (leafs of the Octree) are the only nodes storing mapping information, all other ones containing references to children only. Therefore, the memory occupancy in bytes can be expressed as:

[TABLE]

where $B_{leafs}$ and $B_{inner}$ are the occupancy in bytes of leaf and inner nodes, and $n_{leaf}$,$n_{inner}$ the number of leaf and inner nodes corresponding to non-empty space, respectively. Accordingly, the memory footprint is dependent on the amount of space actually occupied and not on the overall size of the environment.

However, using an Octree rather than a 3D grid implies a space vs. time trade-off, the memory footprint is reduced at the expense of the query time, the computational complexity of a random voxel access being just $\mathcal{O}(1)$ for a 3D Grid, as large as $\mathcal{O}(\log d)$ for an Octree ($d$ denoting the depth of the tree). This issue has motivated a recent proposal by Labschutz et. al.[8] who mix the two approaches into a novel data structure referred to as Jittree and fully managed by the GPU.

Researchers have also explored other solutions, such as Multi-Level Surface Maps [9] and Multi-Volume Occupancy [10], aimed at ameliorating the memory efficiency of the data structures, referred to as height or 2.5D maps, that endow a 2D Grid with measurements concerning the height of obstacles. In particular, information about occupied and free-space is accounted through a dynamic 2D grid where each element is a list of voxels. Thus, akin to Eq.2, the memory occupancy may be expressed as:

[TABLE]

where $x,y$ are the sizes of the projection of the workspace onto a plane (e.g. the ground plane), $r$ the resolution of the above mentioned 2D grid, $B_{tile}$ is the occupancy in bytes of a tile of the grid, $B_{voxel}$ that of each element of a voxel list, $n_{voxels}$ the number of voxels dealing with non-empty space. This kind of approach compares favorably w.r.t. the Octree in terms of memory footprint [10], though, again, at the expense of time complexity: as the data structure is basically a linked-list on top of a grid, a random voxel access takes $\mathcal{O}(n)$ ($n$ being the number of voxels in a list).

As already mentioned, a basic 2D Map built from planar range sensors is ofter not reliable for navigation due to lack of information concerning the height of obstacles. On the other hand, due to the complexity of pursuing path planning directly in the 3D space, most proposals, such as [11][12][13], deploy the rich information embedded into a 3D map so to create a reliable 2D projection that is actually used for the sake of planning. Consequently, the time efficiency of visiting both the entire 3D Map as well as the local neighborhood of a given 3D point (aka radius search) are crucial aspects in the selection of the right data structure.

The mapping framework proposed in this paper, dubbed SkiMap, features a memory footprint similar to an Octree when dealing with large environments and a better time complexity, i.e. $\mathcal{O}(\frac{\log n}{k})$, thanks to a highly-parallelizable computational structure. Besides, it inherently embodies a 3D, 2.5D and 2D map that can be delivered with a time complexity which decreases alongside the richness of the representation. Moreover, unlike the previously mentioned mapping frameworks proposed for robot navigation, SkiMap can deploy the ability of the sensor localization module to deliver optimized poses in order to update the map in real-time. Indeed, such a task is pursued nowadays only by state-of-the-art SLAM systems, such as [14] or [15], aimed at producing high-quality 3D scans of the workspace and meant to run on desktop computers equipped with high-performance and power-hungry GPU cards, an application domain quite different from mobile robotics which calls for compact, low-power computing platforms mounted on-board. In particular, the recent proposal in [15] represents the 3D Space using VoxelHashing [16], which is a memory-efficient data structure managed by the GPU which enables fast random voxel access, but turns out to be vastly inefficient for radius search, which is a key requirement for robot navigation.

III SkiMap MAPPING ALGORITHM

In this section we explore the SkiMap algorithm in its entirety, describing the key data structure as well as how to carry out mapping differently from standard approaches like Octree or 3D Grid. Furthermore, we highlight the inherent parallelism of the proposed data structure, which is conducive to notably improved time efficiency in key tasks dealing with robot navigation.

III-A DATA STRUCTURE: TREE OF SKIPLISTS

SkiMap relies on the basic concept of grouping voxels within a tree as outlined in Figure 1. The actual voxels are nodes at depth 3, which are grouped into nodes at depth 2 according to equal quantized $(x,y)$ coordinates, the nodes at depth 2 in turn grouped into nodes at depth 1 according to equal quantized $x$ coordinates. However, adopting a classical tree structure to realize the concept illustrated in Figure 1 would not be efficient because of the unbounded number of siblings at each depth level (unlike the octree, in turn, where each node has always 8 children). Indeed, should the children of each node be stored in a ordinary list, performing a random access would exhibit $\mathcal{O}(n)$ complexity. To overcome this efficiency issue, we adopted a rather uncommon data structure called SkipList and proposed by Pugh et. al. [17]. As shown in Figure 2 a SkipList is apparently similar to an Ordered Linked List, but the former also stashes a super-structure aimed at bringing the computational complexity associated with random access from $\mathcal{O}(n)$ down to $\mathcal{O}(\log n)$. In a SkipList elements are kept ordered, and thus, compared to an ordinary list, insertion time grows from $\mathcal{O}(1)$ to $\mathcal{O}(\log n)$ due to each insertion requiring a search.

Figure 3 shows the actual realization of the concept illustrated in Figure 1. A first SkipList keeps track of quantized x coordinates, thereby realizing depth level 1 of Figure 1; the items of the first SkipList are referred to as xNodes and colored in red in Figure 3; each xNode is in turn a SkipList which keeps track of quantized y coordinates, thus implementing depth level 2 of Figure 1; the items in these nested SkipLists are dubbed yNodes (green) in Figure 3; eventually, each yNode is a SkipList of zNodes (blue), which represent the actual voxels and provide the containers for any kind of user data. Therefore, the concept shown in Figure 1 is realized by a novel data structure that may be thought of as a Tree of SkipLists. It is worth pointing out that with the proposed data structure the coordinates of a voxel can be obtained by iterating through its predecessors and thus need not to be stored in the containers together with user data; for example for the voxel referred to as $z_{3}$ in Figure 3, iterating back through predecessors provides coordinates $(x_{0},y_{1},z_{3})$. A similar technique is used in Octomap to avoid coordinates storage in the leaves of the octree [7].

As a detailed description of the SkipList is outside the scope of this paper and can be found in [17], we conclude this section with a brief review of the key concepts related to this topic. A SkipList is a multi-level linked-list in which the first level is a list containing all the elements ordered by a Key (each node being a pair $<Key,Value>$) level $i$ contains about half elements of level $i-1$, still ordered by Key. Similarly to a Binary Tree, a search is performed starting from level $i=i_{max}$ down to $i=1$ in $\mathcal{O}(\log n)$, at the expense of memory footprint (due to replicated elements).

Depth (i.e. number of levels) is a parameter of the SkipList to be chosen based on application settings. Indeed, there exists an upper limit beyond which one gets no further benefits in terms of timing performance while significantly increasing memory footprint. As reported in Table I, this is vouched also by our experimental findings.

III-B VOXEL INDEXING

As each node of our data structure is addressable by a Key, we can use it to map real world coordinates to quantized indexes just as it would happen in a 3D Grid. Thus, to retrieve the voxel $\mathbf{v}(I_{x},I_{y},I_{z})$ corresponding to a 3D point $\mathbf{p}(x,y,z)$:

[TABLE]

$r$ denoting, as usual, voxel resolution. Unlike a 3D Grid, however, we can use also negative indexes as they represent Keys of a map rather than simple indexes of an array. This is important for mapping applications as, more often than not, the ground reference of the map (aka Zero Reference Frame) is not known a priori.

With our data structure, querying for a voxel $f(I_{x},I_{y},I_{z})=\mathbf{v}$ consists in executing the iterative query $h(g(f(I_{x}),I_{y}),I_{z})=\mathbf{v}$. Thus, with reference to Figure 3:

• •

$f(\bullet)$ retrieves a red tile / xNode

• •

$g(\bullet)$ retrieves a green tile / yNode

• •

$h(\bullet)$ retrieves a blue box / zNode / Voxel

Each of three query function $f(\bullet),g(\bullet),h(\bullet)$ can result in either a Hit or a Miss. Moreover, each generic function $\phi(f(I_{x}))$ may be performed concurrently because it involves separate branches of the SkipList Tree (see again Figure 3).

III-C PARALLELIZATION

As highlighted in the previous section, the proposed data structure inherently provides for a high degree of parallelization. Besides, even a single SkipList may enable a certain level of parallelization by using locks on nodes [18]. However, we decided to exploit only the high parallelism among voxel indexing operations enabled by our data structure while not deploying also the lock-based technique to further parallelize accessed within a SkipList, mainly to maintain a lean and simpler code and secondly due to lock-based algorithm being often unpredictable, which makes them unsuited to real-time tasks.

As already mentioned the operations involving separated branches of the first level of our SkipList Tree, that is $f(I_{x_{i}})\neq f(I_{x_{j}})\rightarrow x_{i}\neq x_{j}$, can be performed in parallel, We can classify all the possible operations on the data structure into two main categories:

• •

Visiting Operations: Visiting the whole tree (i.e. reaching each voxel of the map) consists in visiting all first-level nodes in parallel and collecting the results:

[TABLE]

• •

Updating Operation: upon performing a generic update operation we cannot know in advance whether it will produce a new allocation or a deallocation or it will just update the content of an existing voxel without performing further search. To ensure that an update operation is not concurrent over others we can reuse again the previous technique: we assume that two update operations do not conflict if they belongs to two separate first level branches. In a typical application scenario we are given a set of sensor measurement to be integrated into the map, i.e. a set of 3D points: $C=\{\mathbf{p}(x,y,z)\}$. Hence, we can group these points in subsets according to their first quantized coordinates

[TABLE]

so to perform the integration operation dealing with each of the subsets in parallel while ensuring no concurrent memory access.

This kind of parallelization is useful not only to improve timing performance but also in scenarios in which map updates may occur from separated sensors in separated chunks, for example in multi-agent localization and mapping [19]. For the record, parallelization across yNodes is also possible but may lead to a computational overhead due to only xNodes being extensive. Nonetheless, we plan to investigate on a possible deeper parallelization of the computation.

III-D NEAREST NEIGHBOR AND RADIUS SEARCH

In a SkipList the Nearest Neighbor Search is straightforward: when we search for a Key in the list we always know the previous and next Keys present in the set, even when the searching Key is missing. A Radius Search around a target index is performed collecting all the elements between two indexes $I_{r^{-}},I_{r^{+}}$ obtained starting from a center index and computing the boundaries with discrete radius dimension:

[TABLE]

as a SkipList is an ordered linked-list, iterating from $I_{r^{-}}$ to $I_{r^{+}}$ allows for executing the search with $\mathcal{O}(k)$ time complexity, $k$ being the number of elements within the range (Range Search). We can extend this approach to each of the SkipLists present in our Tree, so to perform a Range Search along each $x,y,z$ dimension and obtain a Box Search. Then, filtering all the points found within the Box based on the distance from the box center allows for fetching a Sphere and thus achieve a Radius Search. As it will be shown in Section IV, thanks to the parallelization approach enabled by our data structure and discussed in previous section, our method outperforms standard implementation of search algorithms such as the KD-Tree or Octree.

III-E MAP UPDATE ON POSE GRAPH OPTIMIZATION

The idea of Erosion of past sensor measurements and Fusion (or Integration) of new ones in a voxel grid was first introduced by Fioraio et. al. [14]. The integration procedure, described by Curless et. al. [20], allows to fuse sensor measurement in a voxel grid according to a weight; for example, to integrate the occupancy probability:

[TABLE]

where $P^{\prime}(v),P(v)$ are the new and old occupancy probability of voxel $v$, respectively. $W^{\prime}(v),W(v)$ the new and the old weight. As proposed in [14], the Erosion process consists in just inverting the integration process:

[TABLE]

Erosion and fusion of sensor measurements may be deployed in conjunction with any sensor localization module capable of delivering optimized poses, e.g. upon detection of a loop closure. Thereby, the map may be updated by removing sensor measurements according to old poses and fusing them back according to the new, optimized poses. Our mapping system supports this feature by a weight field and a generic data type associated with each voxel, which allows the user to handle any desired kind of measurement (Occupancy Probability, SDF, RGB ….) in order to implement equations 8 and 9.

However, though a sensor tracker typically produces poses at a certain controlled and approximately fixed pace (e.g. at every new sensor measurement or a controlled subset of them), optimized poses are delivered asynchronously with respect to such a regular rhythm, e.g. because a loop closure has been detected, and may happen to compete with live tracking as concerns updating the map. Therefore, as illustrated in Figure 4, we have endowed SkiMap with a Pose Manager capable to create a Pose History: the system treats live poses and optimized poses seamlessly by inserting them in a set of queues, each associated with the sensor measurements (e.g. a depth image) taken at a certain time stamp; a Pose Integrator chooses from the Pose History a subset of poses and integrates the associated sensor measurements in the voxel map; if the pose that’s about to be integrated is an optimized one, its predecessor will be eroded from the map first. The choice of the subset of poses to be integrated into the map occurs according to the following criteria:

• •

live poses must be integrated as soon as possible.

• •

among optimized poses, those spatially closer to the current live pose are picked first.

• •

the upper bound of the subset cardinality is fixed to ensure predictable computation time.

III-F *GROUND TRACKING AND 2D QUERYING *

Although our proposal may be considered a generic 3D Mapping framework, it has been conceived to address robot navigation scenarios. Therefore, we found it useful to endow the framework with a module dedicated to tracking the ground plane. Thus, upon activation of the ground tracking module, the camera mounted on-board the robot must get a shot of the ground plane in the very first frame. The main plane found in the first frame is treated as the ground plane, which allows for classifying easily all the 3D points sensed in the successive frames as either ground or obstacle points. This technique permits also to set the Zero Reference Frame of our map in the centroid of the first floor, thereby ensuring that $z$ coordinates are zero near the ground. More generally, if the core SkiMap algorithm may be provided with measured points classified as ground or obstacles, they can be integrated in the map differently, and, in particular, so as to reduce the time complexity to integrate the former. Indeed, with reference to Figure 3, integrating a ground point boils down to allocating or updating only a green tile/yNode rather than a voxel, which implies reaching just depth level 2 of our SkipList Tree, whilst integrating obstacle points would require going deeper to reach level 3.

We can make the same point as visiting through the SkipList Tree: should we wish to retrieve only the information about ground in order to obtain a 2D Map we would need to visit the tree only up to depth level 2, thereby reducing time complexity dramatically as vouched by Figure 8 . The figure shows also that the ability to create extremely rapidly a 2D view of the 3D Map is peculiar to SkiMap, a classical approach like the Octree being much slower due to the need to visit all voxels and project them on the ground in order to retrieve a 2D Map.

III-G IMPLEMENTATION DETAILS

SkiMap is implemented in C++ and wrapped in a ROS package, so to maximize its portability and usability in the robotics community. Thanks to widespread use of C++ Generics, the SkiMap data structure is contained in a couple of header files. Furthermore C++ Generics enable to chose Data Type to represent coordinates: for example, in our current implementation we have chosen short as index data type allowing values in range $[-32.768,32.768]$, which results in a map of $655.36m$ along each dimension with a resolution of $0.01m$. Also voxels are templetized so to allow the user to store whatever information therein.

IV RESULTS

The SkiMap mapping framework has been evaluated using some heterogeneous datasets categorized as follows:

• •

Medium-sized datasets captured with RGB-D sensors [21].

• •

Public large-sized datasets captured with laser scanners mounted on pan-tilt units (Freiburg Campus 222Courtesy of B. Steder, available at http://ais.informatik.uni-freiburg.de/projects/datasets/fr360/, New College [22]).

• •

Small and Medium-sized datasets captured in our Lab through RGB-D sensor on mobile robots (Figure 10).

The public datasets are endowed with ground truth camera poses, while in the experiments concerning our datasets we deploy Slamdunk[2] to track the camera. Thus, the quantitative evaluation reported in Figures 8, 8, 8, 8 deals with the first two categories only - because of the availability of ground poses - and concerns a comparison between SkiMap and the Octree333version used: https://github.com/OctoMap/octomap that is, to the best of our knowledge, the foremost mapping solution in terms of memory efficiency. To attain a more comprehensive assessment, for each dataset we have considered multiple map resolutions, i.e. $0.05m$, $0.1m$ and $0.2m$. In Figure 8 we have considered also the kd-tree444version used: http://pointclouds.org/ because of its wide adoption in spatial search tasks such as radius search. All the experiments have been run on a 5th generation Intel Core i7.

First we have assessed basic tasks like “Integrating New Measurements” (Figure 8) and “Visiting the Map” (Figure 8), finding out that SkiMap is almost always more efficient than the Octree. Figure 8 highlights how the 2D Query feature introduced in III-F enables to outperform the Octree in obtaining a similar representation. A qualitative example of the 2D Query feature can also be seen in Figure 10, with the ground correctly reconstructed; it is worthwhile pointing out here that, as vouched by Figure 8, obtaining this kind of representation by performing per-voxel projection to ground would imply a significantly higher time complexity. Finally, Figure 8 is about the timing performance of the radius search task, quite relevant, e.g., for the sake of avoiding obstacles while navigating within the workspace under reconstruction Figure 8 points out the much higher efficiency of SkiMap with respect to both Octree and kd-tree, even without considering the initialization time to build the index required by the kd-tree which is not accounted for in the Figure. As for memory occupancy, Table II highlights how SkiMap tend to be almost as efficient as the Octree in case of large environments while providing less memory savings with smaller workspaces.

As for the experiments dealing with datasets taken in our Lab, we used two mobile robots, namely Youbot [23] and Tiago 555http://tiago.pal-robotics.com/, equipped with a Asus Xtion RGB-D sensor and, rather than relying on ground truth information, deployed SlamDunk[2] to track the robot/camera 6-DOF pose and fuse sensor measurements into the map according to the estimated poses. Furthermore, leveraging on the Pose Optimization module offered by SlamDunk, we can realize the Map Update feature of SkiMap (see Section III-E). Both robots were operated manually, in small (Youbot) and medium (Tiago) sized environments within our Lab, so to collect and fuse together multiple sensor measurements in order to reconstruct a map of the explored workspace. Figure 10 depicts examples of reconstructed maps with and without the Map Update process enabled by SlamDunk’s Pose Optimization module. It is worthwhile pointing out that with our approach the optimized maps are not attained off-line within a post-processing step but built in real-time as described in III-E.

V CONCLUDING REMARKS

In this work we have described a novel mapping approach mainly devoted to robot navigation. The primary objective was to provide an efficient mapping framework suitable to real-time applications in embedded robotics platforms. Thus, unlike approaches that focus on dense and accurate 3D reconstruction, such as e.g. [15], our method is aimed at building as efficiently as possible the kinds of representation required to support robot navigation effectively. In its current state the framework can also provide some basic form of semantic information, such as telling apart ground and obstacles. We plan to enrich the degree of semantic perception accommodated by SkiMap by incorporating detection of certain object instances [24], e.g. items to be picked or manipulated by the robot, as well as by leveraging on per-frame Semantic Segmentation so to fuse category labels into the map [25], [26].