Affordable Robotic Mobile Mapping System Based on Lidar with Additional Rotating Planar Reflector

This paper describes an affordable robotic mobile 3D mapping system. It is built with Livox Mid–40 lidar with a conic field of view extended by a custom rotating planar reflector. This 3D sensor is compared with the more expensive Velodyne VLP 16 lidar. It is shown that the proposed sensor reaches satisfactory accuracy and range. Furthermore, it is able to preserve the metric accuracy and non–repetitive scanning pattern of the unmodified sensor. Due to preserving the non–repetitive scan pattern, our system is capable of covering the entire field of view of 38.4 × 360 degrees, which is an added value of conducted research. We show the calibration method, mechanical design, and synchronization details that are necessary to replicate our system. This work extends the applicability of solid–state lidars since the field of view can be reshaped with minimal loss of measurement properties. The solution was part of a system that was evaluated during the 3rd European Robotics Hackathon in the Zwentendorf Nuclear Power Plant. The experimental part of the paper demonstrates that our affordable robotic mobile 3D mapping system is capable of providing 3D maps of a nuclear facility that are comparable to the more expensive solution.


Introduction
Affordable 3D scanning technology is very important for autonomous mobile robots [1]. In recent years, many advantages of 3D lidar for mobile robotics, especially based on solidstate lidar, have become evident. Thus, the overall cost of such a device has been decreasing over time. An interesting research period related to the RGB-D Kinect sensor is worth mentioning [2]. From that moment on, many researchers have studied the possibility of using dense point cloud processing for many robotic applications. One dominant result is the Point Cloud Library [3]. Moreover, it is possible to build an affordable 3D lidar based on rotating 2D lidar [4]. Such approaches have been studied in mobile mapping applications [5][6][7][8]. The advantage of rotating 2D lidar is the use of industrial measurement instruments; thus, many prototypes are sufficient to cope with extreme environmental conditions. The disadvantage is related to the need to move to a relatively high mass; thus, the mechatronic design is rather expensive. Adding a mirror or prism addresses this disadvantage [9][10][11][12]. Unfortunately, an additional reflector changes the physical parameters (e.g., range) of the lidar beams.
Other types of such devices include multi-beam rotating 3D lidars and the recently announced solid-state lidars. Unfortunately, recent solid-state lidars have a limited field of view. We address this gap by introducing the results of research related to a rotating planar reflector that efficiently reshapes lidars' field of view. Two aspects are crucial in such an approach: data synchronization and calibration. For this reason, we demonstrate our framework for solving these important aspects. The added value of our research is that we perform experiments assuming realistic conditions.
Reshaping the field of view of the depth cameras and laser scanners introduces new applications and challenges at the same time. An example of such work is presented in [13], where the authors equipped a depth camera with two angled mirrors to obtain depth perception at both the front and back of the robot. The authors proposed a calibration method based on the graph SLAM g2o framework [14]. Application information, designing, and solving graph SLAM problem are discussed thoroughly in [15][16][17][18]. An alternative solution [19] employs mirrors for adding virtual RGB-D cameras to the mapping system. The authors develop a method for virtual and real camera calibration.
Another interesting study of the design of a radially reshaped field of view is discussed in [20]. It shows great potential for new applications. The authors focused their research on the analysis of optical aspects and scan patterns, but they did not touch geometrical calibration. The authors also described reshaping the radial field of view into a new narrowed one. The problem of such calibration is not discussed [21], but we addressed it in our previous work [22], which is extended for the calibration of the 3D sensor presented in this paper. The idea of reshaping a field of view using mirrors or prisms can be implemented in several applications. Such a solution enables wider perception for unmanned aerial vehicles with a minimal increase in weight [23]. Another example is shown in [24], where such an approach is utilized in the self-driving vehicle domain.
Data calibration based on multi-view data registration is related to the SLAM [25,26] that corresponds to the so-called "chicken and egg dilemma"-the robot or other agent needs to simultaneously localize itself on the map that it is building. It is evident that by having a proper map, we can localize easier, and by having accurate localization, we can reconstruct the map. SLAM problems are divided into two groups: front-end and back-end [14]. The front-end SLAM problem is solved for a map and trajectory that is locally consistent; it consumes raw sensors data, performs feature extraction, and optimizes new poses that are added to the SLAM system. The front-end SLAM cannot adjust the trajectory and map that was obtained in the past; it only affects the most recent nodes of the trajectory. On the other hand, back-end SLAM is used to optimize the whole SLAM system. The back-end SLAM problem is formulated using the trajectory from the front-end and some other information, such as the detection of loop closure. Loop closure is detected when the robot revisits a previously mapped part of the environment.
The paper is organized as follows: Section 2 describes methodological aspects of the calibration procedure, Section 3 shows the mechanical design of the rotating planar reflector, Section 4 explains the synchronization aspects, and Section 6 presents our findings and concludes. An open-source project (example data sets and source codes) is available in [27].
The idea of reshaping the field of view with mirrors or by applying motion to the lidar sensor is common. Our contribution is an optimization method that enables one to automatically calibrate such a device, as well as the mechanical design and strategy for precise time synchronization for modern Livox lidar. Finally, we performed a comparison of the state of the art solution and validated calibrated system against the ground truth data.

Calibration
Calibration of the affordable 3D scanning system composed of a lidar and a rotating planar reflector is essential to preserve measurement accuracy compared to the original lidar. The planar reflector changes the direction of the measurement beam; therefore, it is necessary to assume the pose of its rotation axis to recalculate the measurement point (shown in Figure 1). The goal of the calibration procedure is to minimize the distance between measured points and ground truth points. Ground truth points in this paper were acquired using a Z+F IMAGER 5010 system, providing much more accurate 3D measurements than our affordable lidar. The calibration procedure assumes a modified iterative closest point approach that incorporates the intersection of a reflective plane and the beam into an optimization problem. The multi-view approach is incorporated; thus, measurements from multiple stations contribute to the calibration result. The goal of the calibration procedure is to minimize the sum of errors squared between measured and ground truth points obtained woth TLS.

Iterative Closest Point
This section describes an observation equation for building iterative closest point optimization. Equation (1) transforms point P l (x l ,y l ,z l , 1), expressed in local coordinate frame, to global coordinate frame P g (x g ,y g ,z g ) by a matrix [R,T].
The iterative closest point observation equation is given in Equation (2). It is composed of residuals x δ y δ z δ , target values x gt y gt z gt being ground truth points, and the model function Ψ [R,T] , incorporating state vector β into the optimization process.
Comparisons and further discussions can be found in [36][37][38][39][40]. The final optimization problem is defined in Equation (3), where there are C pairs of points (ground truth and measured) contributing to the optimization process. It is straightforward to extend this into a multi-view optimization problem.

Intersection of a Reflective Plane and the Beam
The rotated planar reflector calibration procedure requires the extended iterative closest point observation equation. Figure 2 shows a geometrical property of the reflective plane intersection and the beam b d . It is marked as r d , and it relates to the measurement. The point (x, y, z) of the intersection of lidar beam b d with a reflective plane V pl satisfies Equation (4).
and d is the distance from the origin to the rotated plane. The lidar beam has origin The intersection P int of the beam b d with the reflective plane V pl is given with Equation (6).
where (·) is the dot product. Due to the rotation of our planar reflector, the plane V pl is transformed using the following equation: where [R c,r , T c,r ] transforms the planar reflector via extrinsic calibration matrix [R c , T c ] and rotation via encoder angle [R r , T r ], assuming the following matrix multiplication: can be computed with the following: Thus, the plane (planar reflector) transformation equation is formulated as follows: The directional vector r d of the reflected beam b d is given by Equation (10). V pl is the normal vector of the rotated reflecting plane.
Point P l (x l ,y l ,z l ) is given in the local coordinates of the lidar. l p = P l is a measured distance in the local coordinates of the lidar. The distance from origin to intersection is l int = P int . Finally, measurement point P r , after applying reflections, is given with Equation (11).
The expected measurement point in the global reference frame is given as Equation (12).
This is used to form an observation equation assuming the reference ground truth point. Finally, the calibration optimization problem is defined as Equation (13).
For the quantitative evaluation of the 3D lidar's accuracy, we used ground truth data obtained by a terrestrial laser scanning Z + F IMAGER 5010 system. The reference point cloud was obtained with high precision. The tool allows one to obtain range measurement with a constant uncertainty of 1 mm with a variable of 10 parts per million. For a maximum range of 178 m, the maximum range error declared by the manufacturer is no more than 3.8 mm. Expected precision degrades with lower reflectivity. The device measures two angles of the ranging beam, both with a variance of 0.007 degrees. Underground parking is considered as the testing scenario, where five stations were used for static data acquisition ( Figure 3). Two of them were used for the calibration procedure using observation Equation (13); thus, we obtained the following parameters: R 2 , T 2 , R 3 , T 3 , R c , T c , a r , b r , c r , d r . R 2 , T 2 is the pose of station 2 and R 3 , T 3 is the pose of station 3. All data from both stations contribute to R c , T c , a r , b r , c r , d r . To confirm the accuracy, we performed data registration of another three stations to ground truth using an optimization problem defined with Equation (3) and fixed calibration parameters from previous calculations. Thus, for each station, the 3D lidar points x l i , y l i , z l i are transformed to the ith local coordinate system R i , T i , i ∈ 1, 4, 5. In this procedure, we obtain three poses as R 1 , T 1 , R 4 , T 4 , R 5 , T 5 . Next, the number of planar features was manually extracted from both the point cloud obtained from the terrestrial laser scanning and the aligned point cloud obtained by the presented system. The TLS (terrestrial laser scanning) data was used to fit a plane. The plane was fitted by performing principal component analysis on points that belong to the planar feature. The basis vector that corresponds to the smallest eigenvalue was taken as a normal vector. Next, the identified plane was used to assess the accuracy and precision of the tested system, as summarized in Table 1. One of the planes is shown in Figure 4.  Columns µ tls and σ tls in Table 1 consist of parameters of normal distribution of distances to the plane. Note that the mean of the distribution µ tls for TLS data is zero, which is expected since the plane was fitted to that set. The standard deviation observed is larger than the error expected from the range measurement of the TLS. That is caused by a number of factors: the planar feature might have low reactivity, or it might not be ideally planar. The map from TLS consists of multiple scans that were aligned manually using specialized software, which can further contribute to the observed value of σ tls . Assuming that TLS provides accurate and precise data, detected planar features can be used to assess both the accuracy and precision of the investigated solutions. For every identified feature, the normal distribution of distances to the plane of observations is described with standard deviation σ livox and mean µ livox . Observed mean µ livox is significantly larger than µ tls for every plane. The same is true for σ livox , which is significantly larger than σ tls . The different values of µ livox for different planes show that the data provided by the investigated system are expected to include some inaccuracy. That observed inaccuracy can be caused by distortion caused by imperfect calibration or the data registration. The standard deviation σ livox also varies with different features: it is significantly smaller for features that are observed closer. The cumulative metric was computed by merging all measurements and down-sampling those to obtain a consistent sampling rate. This was required since a feature that is far away has a significantly lower density of points that hit that feature.

Mechanical Design
The system is mounted on the facing side of the lidar. The lidar is assembled into a faceplate with multiple M3 screws. A BLDC motor, encoder, and axis of rotation lay on an optical axis of the lidar. The circular mirror is tilted by 52.5 degrees to the side. The mirror reflects the rays emitted by the lidar, as shown in Figure 5. The current rotation angle is measured by an incremental, contactless encoder that consists of a stationary head and a spinning ring. The mirror, its support, and the encoder's ring are mounted on top of the BLDC motor. The BLDC motor is a low-RPM, high-torque disc motor, without Hall's effect sensor. The motor is assembled in the housing, which precisely orients the motor with the spinning ring against the encoder's head. That housing is mounted to the top plate. This plate is connected to the faceplate with three pillars. A prototype is shown in Figure 6. Due to the lack of Hall's sensor and usage of an incremental encoder with zero pulse, the device needs to perform an automatic startup procedure. First of all, the motor controller identifies the direction of the magnetic field of the motor's rotor by applying an alternating current to its phases and observing a change in position measured by the incremental encoder. The next system performs so called homing. The mirror is spun until the encoder's zero pulse is found. With those two steps, the system is ready to be used.

Electronic Design
The electronic design of the system consists of a microcontroller with multiple extra components utilized on a PCB (printed circuit board): • Advanced motor controller integrated with on board CAN Open network; • Ethernet switch; • RS485 conntroller (for Livox Mid-40 sync port) The microcontroller firmware is a program that communicates with the host computer via robust, stateless UDP (user datagram protocol). It reports, with a frequency of 1 kilohertz, a current rotation angle with the timestamp. The firmware generates a physical, electric PPS (pulse per second) signal that is used to synchronize external devices, such as the Livox Mid-40. When the internal timestamp generator changes to the next second, a short pulse (50 milliseconds) is generated on PPS output and sent to the Livox Mid-40. According to the Livox Mid-40 documentation, the device on the rising edge of the PPS signal zeroes its internal timer. This timer's count is attached to every packet sent by the Livox Mid-40. The Livox Mid-40 has a simplified synchronization mechanism that only consumes PPS signal. More advanced lidar solutions (such as Velodyne VLP 16 or multi-lidar system with Livox Hub) also consume so-called NMEA (National Marine Electronics Association) stream. This one-way serial communication from a GNSS receiver (or a synchronization device) sends a UTC (Coordinated Universal Time) timestamp in its stream. In the Livox Mid-40, this is not available, so a solution for a robust synchronization strategy is proposed. The Livox Mid-40 and the system's microcontroller send two asynchronous UDP streams with high frequency. A data acquisition application buffers those two streams into the following: • A timely sorted set for encoder (an angle with a timestamp), fixed size, around 0.5 s of a data stream; • An ordinary queue for Livox Mid-40 packets (3D points with a timestamp), with a minimum size of 10 packets, and maximum of 100 packets.
The timestamp that is reported by the system's microcontroller is in a range from 0 to, effectively, infinity (the top plot in Figure 7).
The timestamp that is reported by the Livox Mid-40 lidar is in a range from 0 to 1 s (third from the top plot in Figure 7). The Livox Mid-40 lidar detects the rising edge of the PPS pulse (second from the top plot in Figure 7) and zeroes its timer. Effectively, the Livox Mid-40 lidar reports a fractional part of the timestamp. The major, whole part of a timestamp is reconstructed in the host's software. An extra helper register is used. This register is loaded with the whole part of the timestamp reported by the system's microcontroller, exactly between two PPS pulses (fractional part of timestamp equal to 0.5 s). It can be seen as an initial jump, around sample 50, in the bottom plot in Figure 7. Next, when a falling edge in the timestamp reported in the packets arriving from the Livox Mid-40 lidar is detected, the helper register is incremented. The whole process can be supervised by comparing the current whole second reported by the system's microcontroller and the helper register value exactly between two PPS pulses (fractional part of timestamp equal to 0.5 s). The arriving packet's full timestamp is the sum of the current value of the helper register and the fractional timestamp originally reported by the Livox Mid-40 lidar.
Synchronization software uses those timestamps to attach the Livox Mid-40 lidar's packets to correct the value of the angle read by the encoder. This is done in a separate thread. This thread pops from the queue of the oldest packet and tries to find the best fit in the ordered set of angle-timestamp pairs.

Mapping
The mapping was done by a robot equipped with multiple mapping systems: Livox Mid-40 with rotating mirror, VLP 16, and rotated Velodyne VLP 16, as shown in Figure 8. This robot was used for the evaluation of the proposed contribution. The robot provided two 3D data streams for further comparison. The mapping exercise was done in a mobile mapping fashion with the wheeled platform Clearpath Husky equipped with an AHRS (attitude and heading reference system) using IMU (inertial measurement unit). The wheel odometry and IMU measurements are combined using an EKF filter. Filtered odometry is used to remove the distortion caused by the movement of the robot.
The mapping algorithm aggregates the local, undistorted point cloud for a given distance and arranges its SE(3) (special euclidean group) poses as a variable node in the factor graph. Such individual point clouds are shown in Figures 9-11. It can be seen that the proposed solution provides a larger range of, and similar, metric measurements compared with Velodyne VLP 16 (see Figure 12). Factor graphs are a technique for modeling high dimension non linear optimization problems [41]. They are used in modern localization, filtering, and smoothing robotic applications [42,43]. For modeling and solving our SLAM problem, a state of the art library called GTSAM was used [44]. The filtered odometry at the moment of the beginning of the aggregation is the initial guess for this node pose. The EKF solution is used to undistort the point cloud that was produced during movement. The EKF solution is obtained from the velocity measured by wheel odometry and the attitude provided by AHRS IMU. This is a simple and robust approach that can be used for slow moving platforms in an urban area. This approach is not suitable for a platform where odometry is not reliable or not available, such as surface vehicles or ground vehicles in challenging terrain. In those conditions, lidar inertial odometry should be considered [45]. Figure 9. Undistorted, aggregated point clouds were built using data from the Livox Mid-40 with the rotated reflector (blue) and Velodyne VLP 16 (red) at Zwentendorf NPP. It is important to notice that our system has a larger field of view. Note the non repetitive scanning pattern produced by the rotating reflector.   Next, the factor graph is populated with factor nodes. The first type of factor is introduced by odometry; it is the relative pose reported by the robotic platform. The second consists of AHRS (attitude and heading reference system) measurements, which introduce the measurement of tilt and pitch of the robotic platform. Finally, there are a number of candidate factors that are observed. Such candidates are consecutive lidar measurements and, in particular, lidar measurements that were taken at close distances. This candidate factor includes two aggregated lidar measurements that are matched with a normal distributions transform. The algorithm allows one to perform scan registration utilizing the representation of the point cloud as a grid of Gaussian distributions. When NDT (normal distributions transform) converges, the factor candidate is added to the graph. Such NDT factors create so called laser odometry and loop closure. A diagram showing a simplified version of such a factor graph is shown in Figure 13. The mapping algorithm utilizes data from two types of systems, as shown in Figures 14 and 15. The point cloud using Livox Mid-40 with a rotated reflector utilizes a non repetitive scan pattern, which is visible as curved lines in Figure 9. On the other hand, the Velodyne VLP 16 in this configuration utilizes a repetitive pattern, which is visible as a group of points visible in horizontal bands in Figure 11. Maps built with both system looks similar, but due to their broader field of view, maps built with Livox Mid-40 with a rotated reflector contain features of the environment at a higher level of detail. The non repetitive pattern allowed us to better model the planar features that are visible in Figure 14. Figure 16 demonstrates the undistorted, aggregated point cloud built using data from the Livox Mid-40 with a rotated reflector at Zwentendorf NPP and the distance to the reference point cloud from the Velodyne VLP 16. Both point clouds were compared in the intersection shown in Figure 17. This observation is also supported by quantitative measurements, as shown in Figure 18. It can be seen that the majority of distances are below 10 cm.   The obtained calibration and accuracy were compared against the DEM (digital elevation model) shown in Figure 19. The DEM was obtained from ALS (airborne laser scanning), available publicly in the Head Office of Geodesy and Cartography [46]. In the cross section shown in Figures 20 and 21, the data obtained from the designed system overlap well with the DEM; thus, we consider accuracy to be satisfactory, and the effective range is more than 100 m. This experiment shows that the major discrepancies are around 30 cm.

Conclusions
This paper describes an affordable mobile mapping system based on lidar with an additional rotating planar reflector. The results show that the proposed mobile mapping system can reach comparable accuracy to the state of the art Velodyne VLP 16 sensor and is much less expensive at the same time (the hardware and manufacturing cost of our setup, excluding labor, was approximately 50% that of the Velodyne VLP 16). The solution was evaluated during the 3rd European Robotics Hackathon at Zwentendorf Nuclear Power Plant. It is shown that the Livox Mid-40 lidar with synchronized rotated planar reflector can reach a field of view of 38.4 × 360 degrees without loss of the beam pattern. It is worth mentioning that the rotated planar reflector preserves the beneficial features of the Livox Mid-40 lidar: its large range and non repetitive scanning pattern. The active method of measurement is independent of lighting conditions in comparison to other passive methods such as stereo vision. Further development of the prototype will lead to an investigation of the device in harsh conditions, such as cold or humid weather, precipitation, fog, or smoke. These will be possible when other engineering challenges, such as such proper internal protection, can be ensured. The multi view data registration scheme is proposed, and it can be adapted for other lidar systems' calibration. This work extends the applicability of solid state lidar since the field of view can be reshaped with minimal loss of measurement properties. Thus, it is shown that the proposed method can preserve the metric accuracy. We validated the 6 cm accuracy of the lidar and the range exceeding 150 m. We introduced an affordable mobile mapping system that can compete with state of the art sensors, such as the Velodyne VLP 16. The presented setup enables a few interesting research directions, such as benchmarking approaches in other environments (e.g., underground mining, natural environments) or other domains (e.g., building high definition maps, intralogistics, or agriculture). Our solution provides 3D data with new unique characteristics: omnidirectionality and non repetitive pattern. Those two characteristics can be beneficial in mapping alogorithms without odometry, such as lidar inertial odometry [45], lidar inertial mapping and smoothing [42], or modern approaches to ego motion estimation [47].
Future work will be related to an extension of the evaluation that was carried out in large scale open air nuclear power plant scenarios. One interesting research direction is filling the gaps in DEM with data coming from TLS. Preliminary integration of DEM with data produced by our system is shown in Figure 22. Thus, a variety of different scenarios (such as different climatic conditions, different lighting conditions) will be the core of further research.

Data Availability Statement:
Data is available at [27].

Conflicts of Interest:
The authors declare no conflict of interest.