UFOMap: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown

3D models are an essential part of many robotic applications. In applications where the environment is unknown a-priori, or where only a part of the environment is known, it is important that the 3D model can handle the unknown space efficiently. Path planning, exploration, and reconstruction all fall into this category. In this paper we present an extension to OctoMap which we call UFOMap. UFOMap uses an explicit representation of all three states in the map, i.e., occupied, free, and unknown. This gives, surprisingly, a more memory efficient representation. Furthermore, we provide methods that allow for significantly faster insertions into the octree. This enables real-time colored volumetric mapping at high resolution (below 1 cm). UFOMap is contributed as a C++ library that can be used standalone but is also integrated into ROS.


I. INTRODUCTION
Many robot tasks require a 3D model of the environment to be completed. For navigation and manipulation tasks the model is often used to calculate collision free paths and in exploration to determine where new information can be found and how to get to it. 3D SLAM updates the model and localizes the robot using it.
A number of different methods for modeling 3D environments exists: point clouds, elevation maps [1], multi-level surface maps [2], octrees [3], and signed distance fields [4]. All of these methods are able to represent occupied and free space. However, only octrees and signed distance fields are able to represent the unknown space. Moreover, point clouds, elevation maps, and multi-level surface maps are not able to represent arbitrary 3D environments.
OctoMap [5], one of the most popular mapping frameworks, is based on octrees. It provides a readily available, reliable, and efficient 3D mapping framework. OctoMap provides researchers, also those not focusing on mapping, an off the shelf solution for mapping. While OctoMap excels in many use cases, there exist certain areas where it can be improved. In this paper we focus on two areas.
First, in OctoMap, unknown space is not modeled explictly as occupied and free space. In algorithms where the unknown space is used extensively, OctoMap's implicit representation of unknown space can be a bottleneck. Use cases where this can be a problem are, for example, collision checking, path planning where we want to know if it is possible for a robot to move from one location to another and next-best view exploration methods such as [6], [7], [8], and [9]. In the first two cases the common workaround is to treat unknown space as free space [10], [11]. This way, only occupied space has to be considered in the collision checking. However, this The authors are with the Division of Robotics, Perception and Learning (RPL), KTH Royal Institute of Technology, Stockholm, Sweden. {dduberg, patric}@kth.se increases the probability of collisions. Regarding the third use case, [9] states that "computing volumetric information gains can account for up to 95% of a planner's run-time." The information gain for a certain sensor/robot pose in an exploration scenario is typically calculated as a function of the unknown space. In [7] the information gain is approximated, leading to a reduced need to access unknown space in the OctoMap and it is one of the major contributing factors of the performance improvement over the baseline [6].
Second, manipulating the content of an OctoMap is time consuming and limited. For example, inserting a single point cloud takes on the order of a second. This means that real-time volumetric mapping with an OctoMap is prohibitively slow in many applications. Furthermore, when inserting or deleting data in the map (e.g., mapping) you cannot also at the same time iterate over it to extract information (e.g. exploration). In many use cases the solution is to make a copy of the entire OctoMap structure and let consumers operate on it during the update.
In this paper we present an extension to OctoMap taking these shortcomings into account. The contributions of this work are as follows: Example of an octree. The OctoMap tree representation is shown in (a) and the corresponding tree representation in UFOMap is displayed in (b). Black indicates occupied space, white free space, and grey unknown space. Circles means that it is an inner node, square an inner leaf node, and triangle a leaf node. Note that OctoMap does not make a distinction between leaf nodes types. (c) shows how UFOMap stores that octree in memory.
based on octrees, called UFOMap, that explicitly represents the unknown space, together with occupied and free space. • We introduce different methods of incorporating data into the octree that are significantly faster than OctoMap's. This enables real-time colored mapping at a higher resolution. • We improve the overall efficiency compared to OctoMap and allow for inserting/deleting data and iterate over the octree at the same time. • As opposed to OctoMap, most functions in UFOMap can work at different resolutions of the octree. Making it possible for different algorithms with varying requirements to use the same map representation. • We validate the UFOMap mapping framework in two case studies, volumetric mapping and exploration, and compare it to OctoMap. The remainder of this paper is organized as follows. Related work is presented in Section II, while in Section III the UFOMap mapping framework is presented. Implementation details are given in Section IV, followed by an evaluation of the proposed framework and case studies in Sections V and VI, respectively. Finally, Section VII summarizes and concludes the paper.

II. RELATED WORK
This section gives a brief overview of different 3D map representations used in robotics.
A popular approach to model 3D environments is to discretize the world into equal sized cubic volumes [12], called voxels. One of the major shortcomings of fixed grid structures is that the size of the area to be mapped has to be known a-priori. Memory requirements can also be a problem when mapping large areas at a high resolution. Voxel hashing [13] is one approach to overcome these shortcomings, where fixed sized blocks are allocated on demand.
The mapping framework Voxblox [14] uses a signed distance field [4] voxel grid, with voxel hashing for dynamic growth, as representation. It was mainly developed for planning or trajectory optimizations in the context of micro aerial vehicles (MAVs). The signed distance field representation makes trajectory optimizations faster by storing the distance to the closest obstacle in each voxel. Voxblox builds on [13] where they use a spatial hashing scheme and allocate blocks of fixed size when needed. This means that the size of the area to be mapped does not need to be known beforehand.
One of the most popular mapping frameworks is Oc-toMap [5]. OctoMap uses an octree-based data structure, as proposed in [3], to do occupancy mapping. The octree structure allows for delaying the initialization of the grid structure. It is also more memory efficient compared to Voxblox or a fixed grid structure since the information can be stored at different resolutions in the octree, without losing any precision. If the inner nodes of the octree are updated correctly, it is possible to do queries at different resolutions. This can be especially beneficial in systems where multiple algorithms are using the same map but have different computational and time requirements. These are important properties and we will therefore use an octree representation and build on OctoMap.

III. UFOMAP MAPPING FRAMEWORK
This section presents the UFOMap mapping framework, which is based on OctoMap. We highlight the difference between the two frameworks to make comparison easier.
UFOMap uses an octree data structure, just like OctoMap. An octree is a tree data structure where each node represents a cubic volume of the environment being mapped. A node can recursively be split up into eight equal sized smaller nodes, until a minimum size has been reached. The resolution of the octree is the same as the minimum node size. A visual representation of an octree can be seen in Fig. 2b.
Each node has an occupancy value indicating if the cubic volume represented by the node is occupied, free, or unknown. A probabilistic occupancy value is used in order to better handle sensor noise and dynamic environments. More specifically the log-odds occupancy value is stored in the nodes. Sensor readings can thus be fused in using only additions instead of multiplications. The log-odds occupancy value is clamped to allow for pruning the tree, as in OctoMap. Pruning can be applied when all eight of a node's children are the same. This leads to a smaller tree, which is beneficial both in terms of memory usage and efficiency when traversing the tree.
As mentioned in Section II, the octree data structure allows for queries at different resolutions. This can speed-up applications, as well as allow for different applications that require different resolutions to share the same map. As in OctoMap, a node is defined to have the same occupancy value as the maximum occupancy value of its children. Using the maximum value is conservative and it enables doing path planning, or similar, at whichever resolution without missing any obstacles.
The nodes in UFOMap store three indicators, i f , i u , and i a , which are not found in OctoMap. i f and i u indicates if a node contains free space or unknown space, respectively. No indicator is needed for the occupied space, since the occupancy value of a node is enough to tell if it contains occupied space. The third indicator, i a , indicates whether all of a node's children are the same. i a is useful in cases were automatic pruning has been disabled, but you still want the same behaviour as if it were enabled. Automatic pruning means that the octree is automatically pruned when data is deleted or inserted into the tree. The main reason for disabling automatic pruning is for multi-threading, where you want to access and insert/delete data at the same time (see Section IV-B). It is possible to manually specify when the pruning should occur in this case. The indicators allow for increased efficiency when traversing the tree, by allowing certain branches to be ignored, for example, when only looking for unknown space.
The state of a node, n, in OctoMap is determined by a threshold, t o : where occ(n) is the occupancy value of the node n. In UFOMap this has been expanded to include a threshold, t f , for when a node is free as well: these two thresholds together can be useful in certain applications. In reconstruction or exploration you want to map a certain volume. By setting t f = 0.1 and t o = 0.9, the nodes stay in the unknown state until they have high confidence that they are either occupied or free. This can simplify the reconstruction or exploration algorithm since only unknown nodes have to be considered. In OctoMap's case, by setting t o = 0.9, a discovered node will most likely be classified as free at first. By searching for only unknown nodes in this case the map will likely end up only containing free nodes. The algorithm would therefore have to search through all the free nodes in the map, and usually there are a lot more free nodes than there are occupied nodes. Alternatively, if you use t f = 0.49 and t o = 0.9, the exploration algorithm would focus its attention on the occupied space, meaning you would target higher certainty about the occupied space. This can be convenient in applications requiring reconstruction. Note that you do not have to change anything in the exploration method, and instead use these parameters to modify the exploration behaviour.
Finally, Morton codes [15] are used to speedup traversal of the octree. To generate a Morton code, we first convert the cartesian coordinate c = (c x , c y , c z ) ∈ R 3 to the octree node index tuple k = (k x , k y , k z ) ∈ N 3 0 that c falls into: where d max is the depth of the octree. By interleaving the bits representing k j as shown in Fig. 3, the Morton code m is created for c. From the root node it is then possible to traverse down to the node by simply looking at the three bits of m that correspond to the depth of the child and moving to that child. The Morton code generation has been accelerated by using integer dilation and contraction [16].

IV. IMPLEMENTATION DETAILS
This section covers implementation details and highlights differences compared to OctoMap.

A. Nodes
UFOMap has three different types of nodes; inner nodes, inner leaf nodes, and leaf nodes. In contrast, OctoMap has only what corresponds to the inner nodes and inner leaf nodes.
1) Inner nodes: The inner nodes are all the nodes that have children. An inner node contains a 4-byte log-odds occupancy value and an 8-byte pointer to an array of its children. In UFOMap the children are stored directly in the array, instead of the array holding pointers to the children like in OctoMap. Meaning that 64 bytes are saved for an inner node with 8 children in UFOMap compared to OctoMap. However, this also means that a node in UFOMap either must have no children or 8 children.
The inner nodes also contain the three indicators mentioned in Section III, taking a total of 4 bytes. On a 64-bit operating system this means that the inner nodes require 16 bytes, compared to 72 bytes for OctoMap.
2) Inner leaf nodes: Inner leaf nodes are exactly the same as the inner nodes, described in Section IV-A1. The only distinction is that the inner leaf nodes have no children, meaning the child pointer is a null pointer. Once an inner leaf node gets a child it is considered an inner node. In OctoMap this is the only kind of leaf node that exist.
On a 64-bit operating system an inner leaf node in UFOMap takes up 16 bytes, the same as in OctoMap.
3) Leaf nodes: The leaf nodes are more simplistic. Only the 4-byte log-odds occupancy value is stored. Compared to an inner leaf node, it is not possible for a leaf node to have children. In OctoMap the corresponding node is the inner leaf node, which takes up 16 bytes.
The three kinds of nodes can be seen in Fig. 2c. The nodes which have an occupancy value, indicators, and a child pointer are inner nodes, and where the child pointer is empty is an inner leaf node. The nodes with only an occupancy value are the leaf nodes.
As stated in [5], and also seen in Table I, 80-85% of the octree nodes are (inner) leaf nodes in OctoMap. However, since OctoMap does not differentiate between inner leaf nodes and leaf nodes we see that this value drops to 71-81% in UFOMap, where the distinction is made and only leaf nodes are shown. This means that the majority of the nodes in the tree are leaf nodes. Therefore, having the dedicated leaf node data structures and by storing the children of an inner node directly in an array in UFOMap significantly reduces the memory usage over OctoMap by a factor of 3, even though the total amount of nodes in the tree increases (see Table I).

B. Multi-threading
Multi-core processors are the norm in most segments nowadays. To fully utilize these processors the third indicator i a , mentioned in Section III, which indicates whether all of an inner node's children are the same, was introduced.
When inserting data into UFOMap and OctoMap the tree is automatically pruned when possible. If pruning occurs in a thread at the same time as another thread is iterating through the tree, it can remove nodes that the iterator is currently pointing to. Making the iterator invalid, with undefined behavior if dereferenced.
To prevent this, UFOMap allows for disabling the automatic pruning. In this case i a will be used to determine if a node is an inner node or an inner leaf node. If it is the latter, it will not traverse further down the tree. This allows for inserting data and traversing the tree at the same time, while still utilizing the octree structure. It is possible to manually tell UFOMap when to prune the tree, such that the memory consumption still can remain low while maintaining thread safety.

C. Integrating Point Cloud Sensor Measurements
Ray tracing is used to integrate point cloud sensor measurements. When integrating a point cloud it is important that points that should be occupied gets occupied and that all points between the sensor origin and each point of the point cloud gets freed. In UFOMap there are four methods for this, each faster than the previous but with less accurate results: 1) Simple integrator: For each point in the point cloud we trace a ray, using [17], and free all nodes from origin to the point. We set the node in which the point lies to occupied. Same as in OctoMap.
2) Discrete integrator: We use the fact that a number of points in the point cloud fall inside the same node. So by discretizing the point cloud first, the ray tracing is only done once for each node the points fall into. The ray tracing is performed towards the center of the node which means the result can be different compared to the simple integration. However, this is significantly faster for large point clouds with many points falling into the same node. The same method exists in OctoMap.
3) Fast discrete integrator: In the fast discrete integrator, the ray tracing and discretization is performed at multiple different depths. First, a ray tracing algorithm (Eq. 2), cruder than [17], is applied at a coarser resolution, corresponding to depth d. Ray tracing is done until n nodes, at that resolution, away from the end node. From that node we perform a new ray tracing at lower depth until we reach depth 0 (leaf node depth) where we perform the same ray tracing as in the discrete integration. This ensures that we do not clear space behind the occupied space. The parameters n and d allow us to trade speed against accuracy.
where res d is the resolution at the depth d. The special case d = 0 corresponds to the discrete integrator above and in the case n = 0 space is cleared only at depth d. Extra care has to be taken when inserting data into the octree at a different depth, as in the fast discrete integrator method. When clearing free space at depth d we check if the node at that depth is not occupied. If it is not occupied, we can simply update the occupancy value of that node, and remove its children. If it is occupied, we will instead go down one more depth level and perform the same check until we are at a leaf node.
The occupied nodes are updated at the highest resolution in all three integrators. Thus, the occupied space will look mostly the same for all three methods, which is the most important aspect in most applications.
A comparison of the integrators can be seen in Fig. 4.

D. Bounding Box
UFOMap and OctoMap supports defining a bounding box to speedup operations. Only a subset of the octree has to be searched and updated. When inserting data into OctoMap only rays that begin and end inside the bounding box are integrated into the map. This can be a problem for exploration algorithms which rely on free space being cleared. In UFOMap, this problem is fixed, by moving the beginning and ending of the rays towards each other until they both are inside the bounding box.

E. Accessing Data
To access the data in UFOMap, iterators are used. They are fast and they can be used to specify bounding boxes, bounding spheres and bounding frustums. Only bounding boxes are available in OctoMap. Iterators can also specify the type of nodes that should be returned, e.g., only leaves, occupied, free, unknown, contains occupied/free/unknown, or a combination of them. In comparison, OctoMap always returns free and occupied nodes. It is not possible to get the unknown nodes, and it is not possible to get only occupied nodes or only free nodes.

F. Availability
UFOMap is available as a self-contained C++ library at https://github.com/danielduberg/UFOMap. Packages for integration with the Robot Operation System (ROS) [18] are also available. There are functions for reading and writing Oc-toMap files and converting between UFOMap and OctoMap. This facilitates the transition from OctoMap to UFOMap in an already existing system, and allows to utilize both mapping frameworks for different parts of the same system.

V. EVALUATION
We compare our proposed mapping framework, UFOMap, against OctoMap when it comes to memory consumption, insertion time, and in three different use cases.

A. Memory Consumption and Node Count
In the first experiment, we compare the memory consumption between UFOMap and OctoMap on the OctoMap 3D scan dataset 1 . We analyze the memory usage when the tree has been pruned. Table I shows that UFOMap is around 38% of the size of OctoMap, even though UFOMap contains more nodes in total, as seen in the same table. The increase in the number of nodes is a result of the unknown space being explicitly represented, meaning an inner node must have either 0 or 8 children. The decrease in memory usage is because of i) 1 http://ais.informatik.uni-freiburg.de/projects/datasets/octomap the smaller data structures for the leaf nodes mentioned in Section IV-A and ii) storing the child nodes directly in the child array of the inner nodes, instead of storing pointers to the children as in OctoMap.  Table II, using the cow dataset 2 . For both UFOMap and OctoMap the discrete integrator, mentioned in Section IV-C2, is used. UFOMap uses the fast discrete integrator, mentioned in Section IV-C3, with n = 2 and d corresponding to the depth of the voxels at 16 cm voxel size. UFOMap † uses the fast integrator with n = 0 and again d corresponding to the depth of the voxels at 16 cm voxel size.

Point cloud integration time comparison between UFOMap and OctoMap is shown in
The total time represents the average time for a single point cloud to be integrated. Ray tracing shows the average time per point cloud for doing the ray tracing part of the insertion. Insertion shows the average time per point cloud for integrating the points calculated from the ray tracing step into the octree. The standard deviation is included for all.
When only looking at the discrete integrator for UFOMap, we see that it is about two times faster at the insertion part of the integration compared to OctoMap. The ray tracing is between 1 to 2 times faster in UFOMap, which is, most likely, due to different implementation factors since both utilize the same algorithm. As the voxel size decreases, the difference between the two framework increases, in favour of UFOMap. At a voxel size of 4 cm and below, the need for the faster integrators becomes very apparent. UFOMap and UFOMap † provides just that, fast integration while still clearing free space. With UFOMap † scaling a lot better with the resolution and being 26 times faster than OctoMap at 2 cm voxel size.

C. Collision Checking
In the first of the use cases we check if the robot, or a part of the robot, can be at a certain position in the map. That is, we check if a region of the map is free, meaning there is no occupied or unknown space in that region. This is an operation that is heavily used in sampling-based motion planners, such as RRTs. By not allowing any unknown space in this region we are more conservative and safe. We sampled 1 000 000 poses where at least the center of the pose was in free space. A radius of 25 cm was used. On average around 50% of the poses sampled were in collision. The results are presented in Table III. We can see that UFOMap allows for faster collision checking than OctoMap in all cases but the ones with large voxels. This is most likely because of the overhead for constructing the iterators in UFOMap in these cases. The last column shows the result when using the same way to traverse the octree for collisions as in OctoMap in UFOMap. We see that this is faster than OctoMap for all resolutions. However, it is significantly slower than the default method in UFOMap, which exploits the octree structure, for the higher resolutions.
The reason for UFOMap seemingly being invariant to the voxel size can be because of the indicators i f and i u , presented in Section III, together with the iterators mentioned in Section IV-E. By specifying the bounding box and that only occupied and unknown voxels should be retrieved, UFOMap can move straight down the octree to a node that is either occupied or unknown inside the radius.
For comparison reasons we have also included the time taken if only checking a region for occupied space. This corresponds to a simplification often made in collision checking to speed up the computations. The results are presented in Table IV. We can see that UFOMap is faster than OctoMap at all resolutions. Reasons for this can be the use of Morton codes for traversing the octree, perhaps less cache misses since less memory is used, or the inclusion of the indicators. Both UFOMap and OctoMap exploit the octree structure in this experiment. Hence, there is no need for UFOMap octo .

D. Collision Checking Along a Line Segment
In the second use case we perform collision checking along a line. For simple RRT path planning, the operations described in Sections V-C and V-D are combined.
As in Section V-C, we are conservative and require that there is no occupied or unknown space along the line. The line is defined by two randomly sampled points. As seen in Table V UFOMap is between 2 to 15 times faster than OctoMap.

E. Calculate Information Gain
In reconstruction and exploration applications, next-bestview [19] planning is a popular approach. The next-best-view is often obtained by calculating the information gain from being at a specific pose in the map. The information gain is a measure of how much new information can be collected from a certain pose. In exploration, where the goal is to turn each voxel into either occupied or free space, the information gain can simply be how many unknown nodes can be seen from a certain pose. In this experiment we compare the performance when calculating the information gain in UFOMap compared to OctoMap. For the sensor we use a horizontal field of view of 115 o and vertical field of view of 60 o . The minimum and maximum range of the sensor were set to 0 m and 6.5 m, respectively.
The results from this experiment can be seen in Table VI. OctoMap and UFOMap octo compute the information gain similar to how it is done in [6]. Both not exploiting the octree structure.
For UFOMap we exploit the octree structure to quickly find the unknown voxels inside the region of interest. For each node found we do ray tracing from the sensor to the node, at the same depth in the octree as the node. If the node is not blocked by any occupied space we add it to the total gain, otherwise we recurs down to the node's children and do the ray tracing for each child at their depth instead. We do this until the node is either not blocked by an occupied node or until we are at the leaf depth.
UFOMap fast also exploits the octree structure. For each unknown node found, we recurs down to the leaf nodes right away and do the ray tracing. Once a single of the children is not blocked we assume that we can see all children and add all of them to the total gain. Therefore, this approach gives more of an approximate answer than the other.
As seen in Table VI the information gain computation is 1.5 to 15 faster with UFOMap compared to OctoMap, depending on which UFOMap method is used.

VI. CASE STUDIES
In this section we compare UFOMap and OctoMap on two larger use cases.

A. Real-time Volumetric Mapping
In the first test, we compare the performance on volumetric mapping on the freiburg3_long_office_household sequence from [20]. It is a 86 s office sequence with point cloud data at 2 Hz.
UFOMap managed to incorporate all point clouds and create a 3D map with 4 mm voxel size with color information at 2 Hz, seen in Fig. 5. Without color, 2 mm voxel size without missing any of the point clouds were the limit for UFOMap. In both cases the fast discrete integrator were used, mentioned in Section IV-C3, with n = 0 and d = 4. OctoMap were able to run 3 cm, without color, without missing any point cloud. At 2 cm OctoMap could not keep up with the point clouds.
UFOMap allows specifying the depth of the octree, from 1 to 21. The limiting factor being the Morton code, mentioned in Section III, which is specified to be 64-bit. UFOMap can therefore cover at most 2 21 × 0.001 m = 2097.152 m in each dimension, with 1 mm voxel size. OctoMap's depth is in the current implementation fixed at 16 depth levels. Meaning, OctoMap can cover at most 2 16 × 0.001 m = 65.536 m in each dimension, with the same voxel size.

B. Exploration
In the second test, we compare UFOMap with OctoMap in a next-best-view exploration scenario. We have chosen this scenario since it incorporates all of the above use cases in a realistic setting. We have chosen to use the receding horizon next-best-view exploration method, proposed in [6], for this comparison. For each node that is being sampled for the RRT, there is a check if the node is in free space. There is also a check if the path between the newly sampled node and prospecting parent node is clear. Lastly, when selecting where to move next we calculate a score for each node in the RRT. This score depends on the distance and the information gain along the branch to the node.
In UFOMap, as the exploration proceeds the benefit of the proposed approach increases. As more of the space gets classified as free space, all of the above calculations are accelerated in UFOMap compared to OctoMap. This means that when the environment is almost fully explored you can discard nodes that give very little new information quickly. This shows that UFOMap is especially well suited for exploration, compared to OctoMap where the necessary calculations are not affected to the same degree by how much is explored.  The power plant scenario from the Gazebo model library 3 was used for the exploration test, seen in Fig. 1. 16 cm voxel size was used. As can be seen in Fig. 6, switching from OctoMap to UFOMap makes a significant difference to the exploration rate. UFOMap managed to finish the exploration after around 650 s, while OctoMap had completed 70% of the exploration after 900 s, which was the maximum allowed time.
We also investigated running the exploration without mutexes, that is, allowing the mapping and the exploration sides simultaneous access to the map. Both OctoMap and UFOMap crashed. However, UFOMap did not crash when the indicator i a was used. As described in Section IV-B, this indicator turns off the automatic pruning, since nodes are not actually removed from the octree in this case.

VII. CONCLUSION
We present UFOMap, an open source framework for 3D mapping. UFOMap was built on OctoMap, which is one of the basic building blocks in a number of different robotics applications. Just like OctoMap, the underlying data structure is an octree. OctoMap only explicitly models occupied and free space, while UFOMap explicitly models occupied, free, and unknown space. This representation, together with the fact that every node in the octree stores indicators for what kind of space their children contains, results in a significant performance boost compared to OctoMap for use cases where unknown space is extensively used.
Along with these improvements, we introduce new ways of integrating data into the octree. We show that this leads to significant reductions in the time to insert new measurement data, such as point clouds, into the map.
The UFOMap mapping framework is freely available at https://github.com/danielduberg/UFOMap and can be easily integrated with robotics systems. It is written in C++ and can be run as a standalone package or integrated into ROS [18].