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 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 make 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 Figure 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 Figure 2.
• Vectorization: This module contains algorithms for fitting of geometrical primitives to continuous clusters of point. Traditional point-eliminating approaches are covered (Reumann-Witkam [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 Figure 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. Figure 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-to-pose 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 main read-me file also contains instructions on building and running of this supportive code.

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 functions usually do not check validity of their arguments, moving the responsibility on user’s side. Such behaviour is 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 performance subtleties. 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. Table 1 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. According to CLion integrated development environment, the code coverage of the tests is greater than 95 percent.

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. (Ubuntu 20.04 is tested)

Programming language

C++17 is mandatory for use of the library. (tested with CMake 3.15 and GCC 10.0)

Additional system requirements

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

Dependencies

The Eigen library [14] (v3.3.9 is tested) 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 (v1.10.0 is tested).

List of contibutors

• Ales Jelinek – development, architecture, testing and benchmarking
• Adam Ligocki – development, testing
• Ludek Zalud – supervisor, technical feedback

Software location

Language

English