Robotic Template Library

Robotic Template Library (RTL) is a set of tools for dealing with geometry and point cloud processing, especially in robotic applications. The software package covers basic objects such as vectors, line segments, quaternions, rigid transformations, etc., however, its main contribution lies in the more advanced modules: The segmentation module for batch or stream clustering of point clouds, the fast vectorization module for approximation of continuous point clouds by geometric objects of higher grade and the LaTeX export module enabling automated generation of high-quality visual outputs. It is a header-only library written in C++17, uses the Eigen library as a linear algebra back-end, and is designed with high computational performance in mind. RTL can be used in all robotic tasks such as motion planning, map building, object recognition and many others, but the point cloud processing utilities are general enough to be employed in any field touching object reconstruction and computer vision applications as well.


INTRODUCTION
Geometry is ubiquitous in robotics. We need it to describe robot's pose, plan its trajectory, model the environment it is operating in and all of these tasks require a good way of handling rigid transformations, shape representations, data approximations and object registration. While rigid motions are well explored, with rich theory [1] and coverage in most robotic libraries [2], [3], [4], the shape representation and approximation is mostly treated either in the perspective of computer graphics [5], regression analysis [6] or probabilistic robotics [7]. There is nothing bad with either of these approaches, because robotics needs all of them, but there is mostly no simple transition between these views of the same thing [8] and this was the primary motivation of our research in the field geometric data processing. Computational efficiency was also in the centre of attention, because direct implementation of mathematical formulas often does not lead to an optimal algorithm. Robotic Template Library (RTL) development started together with our research of the fast vectorization algorithms and is therefore a repository of code we used to illustrate A large portion of work on the Robotic Template Library was carried out on Brno University of Technology, Faculty of Electrical Engineering and Communication, Department of Control and Automation as a part of the PhD. training of the authors. We are grateful for its supportive environment which makes free development of new ideas possible.
The completion of this paper was made possible by the grant No. FEKT-S-20-6205 -"Research in Automation, Cybernetics and Artificial Intelligence within Industry 4.0" financially supported by the Internal science fund of Brno University of Technology. 1  our results in research papers. It is a known fact, that reproducibility of research in robotics is an issue [9], [10] and we would like to help the initiative to fix it. These algorithms are also what makes RTL truly unique among other similar projects. RTL provides an implementation of the fast total least squares (FTLS) vectorization [11] of the ordered point clouds -an optimized algorithm for approximation of ordered data, which provides computational performance similar to the point-eliminating methods [12], while preserving all the benefits of the TLS regression. An augmentation of the previous approach [13] for optimization of global error is present in RTL as well.
Robotics is a rapidly evolving domain aggregating a large range of scientific and technical disciplines in the pursuit of autonomous -maybe even intelligent -machines. And indeed, this implies there are already many software libraries for general robotics as well as point cloud processing, linear algebra, computer vision and dozens of other tasks. Why yet another library? The reason stems from the purpose of current state of the art libraries in the field. There are large, well established projects overlapping with RTL, e.g the Robot Operating System [2], the Point-Cloud Library [3] and the Mobile Robot Programming Toolkit [4], but these are more focused on mature methods and algorithms for practical problem solving and tend to be too "high-level" for development of new methods we are interested in. For this reason, we have adopted the mathematical library Eigen [14] as a base for our research and built on top of it a standalone project, which is easy to adapt and experiment with. As such, RTL classifies as a small, geometry focused library for evaluation of our research and simple integration into other projects.

IMPLEMENTATION AND ARCHITECTURE
The content of the library is sorted into several modules according to the functionality they provide. Let us first review the content provided and discuss the technical details later. The modules of the Robotic Template Library are as follows: • Core: The module provides base functionality such as constant definitions independent from STL implementation, algebraic primitives such as vectors, matrices and quaternions, as well as representations of the most important geometric objects such as line segments, polygons, bounding boxes, view frustums etc. Especially the linear algebra part heavily builds on the Eigen library and is often only a thin wrapper tweaking object interface with only a few additional functionality. The geometry related objects are implemented as templates for Euclidean space of arbitrary dimension. STL compliant type traits relevant to RTL are present here as well. • Input/Output: This module provides an interface to other formats for storage and presentation of RTL objects. STL compliant command line output is present for logging and the most basic user interaction. The second output possibility are the LaTeX export classes. One of them is dedicated to 2D vector drawings, graphs etc., the other contains a basic renderer of 3D scenes, next one covers basic tables and the last one allows to aggregate multiple figures into a single document and automate compilation of the LaTeX code. Resulting vector graphics can be used to provide high quality visualization of the computations possibly leading to direct generation of reports from experiments such as in case of examples in Fig. 1. All figures and tables in this paper were generated using this module. • Segmentation: Algorithms for point cloud processing into continuous clusters of points are covered in this part of RTL. This procedure is an important step for outlier removal and a guarantee of continuity (according to given parameters) enabling the fast vectorization algorithms described further. There are two variants of the segmentation algorithm: one is suited for circular point clouds from static scans of rotating LiDARs, while the other is designed for processing of a continual stream of input points. Both segmentation algorithms are based on the same criterion, which makes their results easy to combine. An example of 2D and 3D segmentation is given in Fig. 2 [20] and Douglas-Peucker [19] algorithm), but the main focus is on total least squares (TLS) fitting. There is an implementation of the fast total least squares (FTLS) vectorization [11] and the augmentation [13] for optimization of global error. These fast algorithms are used for extraction of approximation lines in 2D and 3D and for planes in 3D (see Fig.3). As the vectorization procedure is separable into several steps, the library is designed to allow modifications of the processing pipeline, which results in many optional features such as approximating polyline construction, or global error optimization. The unit test of these algorithms also includes an extensible benchmark for comparison with other similar methods (e.g. Fig. 4). • Transformation: Geometrical transformations are definitely a core feature of the RTL, however they aggregate so many related objects, that they were moved into a separate module. This module covers the most important basic operations: translation, rotation and rigid transformation of applicable objects from the Core module. All transformation work in general in N-dimensional space and can be composed together.
There is also a mechanism to organize the transformations, because in applications, we usually need to represent a hierarchy of transformations between different coordinate systems. For more general case RTL provides a tree structure, where nodes correspond to poses and edges to actual transformations between them. For simple point-to-point transitions there is a chain structure, which is also returned, when pose-topose transformation is queried from the tree. To allow multiple types of transformations to appear in these structures, there is a generic type allowing to store any transformation desired and a complex treatment of uncertain return types of its application on other objects. • Test: Testing module provides tools for the library examination, however templates for automated testing of instantiation of other templates and random number generation might be handy in RTL applications as well.
There is also a module storing additional information on the data types used throughout RTL such as maximal allowed error in tests or human readable description of the type for logging and error reporting.
The downloadable content of the RTL package also contains two sections with no additional functionality, but still important for the library: • Unit tests: Any larger project needs a validation mechanism of some kind and unit testing was chosen as an optimal solution for the RTL. The tests contain code to examine vast majority of the classes and methods available in RTL, in case of the vectorization module, there are basic benchmarks as well. The most of the tests are implemented using the GTest suit and the test module providing an automated way of testing of the library, but the tests of more complex algorithms in the segmentation, vectorization and LaTeX export modules provide graphical output in a .pdf file for visual examination. From the user's point of view, the tests are useful for examination whether changes to the library did not broke the rest of the code and can serve as a companion to the examples, since they cover RTL more completely. • Examples: Basic usage examples may help to start with RTL faster. In a set of several short and simple snippets, a densely commented code shows how to harness core features of the RTL. Only some core features and the transformations are covered at the time of publishing of this paper, but considerable expansion of this section is expected in future releases.
The Robotic template library is designed to be highly efficient in computational tasks. This is achieved using the Eigen [14] library as a back-end since it provides a very optimized code for general algebra and much more. Usage of templates and static polymorphism saves processing power on run-time and allows stricter type checking during compilation. Default constructors perform no initialization and function usually do not check validity of their arguments, moving the responsibility on user's side. Such behaviour is (a) Visibility diagrams [15].
(c) Pose uncertainty [16].  thoroughly documented and allows to omit redundant checks, when the data are guaranteed to be valid by other means. Of course, higher-level code comes with lower emphasis on every instruction. Transformation management structures are a good example of a balanced approach to ease of use and performance, because highly optimized transformations are expected to be applied millions of times per second, while modifications of the tree are much less frequent and thus allow some more computational costs. Separate case are the LaTeX output classes, where only the most significant optimizations were carried out, since the bottle necks of this output are outside the RTL.
As any other larger project, RTL also maintains coherent structure over its modules, unified code style and naming convention. This is especially useful in combination with the template nature of the library and allows to create a very general code through meta-programming techniques. To aid programmers with such applications, we provide type traits compatible with those from C++ STL. Tab. I summarizes available traits and also shows a portion of RTL in a new perspective of common features and mutual relationships.

QUALITY CONTROL
RTL is distributed with a set of tests, which are used to ensure its proper functionality and can be used as a control mechanism, if the user makes some changes within the library. The tests are separated to focus only on a small portion of the code resembling the principles of unit testing. Some more advanced features of the library expect the more basic parts to be functional, e.g. it is recommended to run the test on vector algebra before the point cloud processing one.
A feedback on proper function of user's code appears in two ways. In case of unit tests, no output means no errors, otherwise a human readable description of what happened is provided. In case of general programming, compile time assertions are verbose as well and run-time behavior should simply correspond to the specification in the documentation available on-line.   Operating system Any operating system capable of compiling and running C++17 code.

Programming language
C++17 is mandatory for use of the library.

Additional system requirements
There are no special requirements. RTL is provided as a header-only library.

Dependencies
The Eigen library [14] and the Standard Template Library (STL) of the C++ language are mandatory. To compile the library tests, GTest is needed as well, however the library itself works without it.  RTL has several modules for different purposes, which all meet in robotic applications. It can be used to build more advanced algorithms such as motion planing, scene interpretation, data fusion, feature extraction, localization, mapping and others, because it contains the base tools for dealing with geometry in space and for processing of point clouds. It is worthy both for development (due to generation of visualizations using the LaTeX export module) as well as for production (because the code is highly optimised).

List of contibutors
However there is no restriction on using only a portion of the library for solving a specific task from a different field.
Geometry is extensively used in computer graphics, physical modelling or even game development. The point cloud processing can find its usage in 3D scanning applications, signal processing and computer vision. LaTeX export is useful in all applications, where vector graphics needs to be generated directly from a program. Especially if the Eigen library is used for computation, RTL can be employed as a graphics output back-end.
Since RTL contains implementation of separately published algorithms, it can also be used in comparative studies, surveys and benchmarks for independent comparison of our work with results of other researchers.
RTL is published with all source codes available under the MIT licence, which makes it freely modifiable to any degree desired. The library is written with modularity and extensibility in mind so adding new features is possible and even encouraged. The code is hosted on GitHub, where all the feedback, bug reports and issue tracking should take place. We do not provide any official support for users of RTL, but we are open to discussion on features and grateful for bug reports, since quality and reliability of the library is our important goal.