Slam algorithm for aruco landmark array based on synchronization optimization

High precision mapping algorithm is the core of two-dimensional(2D) landmark array(the ArUco landmark is used in this paper) localization application. In most of the existing mapping algorithms, the accumulated errors of online methods can not be eliminated, meanwhile, the manual calibration and off-line methods are inefficient and time-consuming. In order to overcome those shortcomings, a real-time simultaneous localization and mapping(SLAM) algorithm for landmark array based on synchronized optimization is proposed in this paper. The proposed algorithm updates and corrects the global map online, and the accumulated errors is optimized based on minimizing the re-projection error of the landmarks ’ vertexes. In order to verify the proposed algorithm, several contrast experiments with other mapping systems are presented. The experimental results show that the proposed algorithm has the advantages of high map precision and small accumulative error.


Introduction
Landmark localization is the most commonly used method in machine vision. It is easier to identify and more reliable than the natural feature, meanwhile, its recognition algorithm is less computational and faster. Therefore, the real-time performance can also be guaranteed on low-cost embedded processor.
There are a lot of landmark systems proposed in the recent years, such as ArUco [1], AprilTag [2] and Whycon [3]. The landmark based localization system can be built in the following three methods: (1) Single landmark location is commonly used in UAV localization and autonomous landing, such as [4][5]. Although this system is simple and easy to realize, but it has the disadvantages of large dead zone and small location range. (2) Compound landmark location uses several landmarks tightly combined to realize the continuous localization in a wide range. In [6], the author adds two small size landmarks to the blank area of the ArUco landmark. In [7], the author uses several small size landmarks around a large one to provide reliable localization data. (3) Landmark array location realizes the precise localization in large space with dozens or hundreds of landmarks, which meets the demand of high precision and low cost localization system nowadays [8]. The core of landmark array localization is its precise map. The maps in [8] and [9] are obtained by manual calibration, but they are inefficient and time-consuming. Therefore, many researchers proposed to use the 3D reconstruction algorithm to realize the automatic mapping. The current automatic mapping system can be divided into offline and online methods. The off-line method generates high accuracy map with complete video and long processing time. For example, the system proposed in [10] can build a precise map based on the camera ' s trajectory obtained from visual odometer. The system proposed in [11] effectively reduce the accumulated errors of map by 2 minimizing the reprojection of landmarks' vertexes in multi-frame. The online method uses the realtime recognition results of landmarks to update and correct the map in real time. For example, in [12], an online mapping system based on extended Kalman (EKF) is implemented. However, its covariance matrix inverse process requires a large amount of calculation. The system proposed in [13] estimates the new landmark ' s pose based on the constraint in coordinate transformation. However, the accumulated errors of map are relatively large due to the lack of global optimization. In [14], an online mapping algorithm based on graph model is proposed, in which the new landmark's pose can be calculated by finding the shortest path in the graph. Although the algorithm is easy to implement, it still cannot effectively reduce the accumulated errors of the map. In summary, there is still room for further improvement in the current automatic mapping system. Therefore, a real-time mapping algorithm is proposed in this paper, which is shown in figure 1 .  Figure 1 shows the proposed synchronized optimization framework, in which contains three parts. The online mapping updates and corrects the global map in real time, meanwhile, the key frame sequence is captured during the online mapping process. The global optimization uses the map generated by the online mapping tread as the initial pose graph to optimize the accumulated errors. Finally, the optimized map is synchronized to the global map. In order to verify the performance of the proposed algorithm, several comparison experiments are presented in this paper and the results show that the proposed algorithm achieves high-precision map and real-time localization.
This paper is arranged as follows: The online mapping algorithm is presented in 2 Section , the global optimization algorithm and the synchronized optimization algorithm is presented in 3 Section. The experimental results and analysis are given in Section 4 and the summary of the paper is presented in Section 5.

Transform matrix mean value method based on nonlinear weighting
Define the landmark coordinate system as{m} , define the global coordinate system as {n} . The rotation matrix R and translation vector t of the camera or the landmark can be represented as the pose matrix ={ | } T R t in SE(3). Thus, the any landmark's pose matrix can be estimated based on the closed principle of coordinate system transformation(CPT) in idealy. In order to improve the map accuracy, remove the outliers and decrease the errors caused by PNP algorithm, a matrix array is built in this paper, which stores all the estimated poses matrixes from different landmarks, and the mean value of the array is used to correct the global map. In order to simplify the algorithm, the mean value of rotation matrix and translation vector is calculated respectively. For the translation vector, the mean value can be obtained by calculating itsarithmetic mean: In order to solve the above problem, the correction vector r is calculated in the SO(3) and obtains its relative rotation matrix m R with the Rodrigues transform method [15]. The vector r can be calculated as follows: Where  is a regularization parameter, and the vector ( ) Where i d is the distance between the camera and the landmark.

Online mapping algorithm
In order to realize the online mapping and correction the following algorithm is proposed here.  is the nonlinear weight,  is the adaptive parameter, and k d is the distance between camera and landmark. The key frame is captured after every move a fixed distance.

Global Optimization and Synchronized Algorithm
In order to optimize the accumulate errors in global map. The following global optimization and synchronized algorithim is proposed when the key frame sequence is full, which improve the map


Where ( )   is the re-projection function, the objective of the global optimization is to minimize the reprojection error of all vertexes in the key frame sequence, thus the optimization functions is : Where m is the number of key frames, n is the number of landmarks, andˆj u is its pixel projection. The global optimization algorithm is given as following:

Initial Pose Graph
The algorithm directly uses of key frame sequence overflow time to construct the initial pose graph ( ) s G t , which is shown in figure 4.

Global optimization algorithm
For the global optimization, the ( ) s G t is optimized by minimizing the re-projection error of the landmarks' vertexes. In ideal case, the loop shown in Fig. 3 should satisfy the CPT method. 4 is a certain weight, which is the mean value of two nodes' re-projection error. Thus, the larger the projection error is, the larger the correction is. The weight ,


According to the graph theory, the total error of one loop can be allocated to its sub-loops [11]. Therefore, the modified rotation matrix between two landmarks , i j is , * , i, j i, j The optimized translation vector needs to minimize the error between * t and ' t , meanwhile, for any point k v still satisfies the CPT method in loop, thus the optimization problem is given as follows: In the same way, the above optimization problem can be built through all the loops in ( ) s G t and constitutes a constrained linear quadratic optimal problem [11]. The LM method is used here to get the optimal * t . Finally, the optimized transformation matrix * T can be calculated and generated the optimized map * ( )

Synchronized Optimizationm
In order to solve the mismatch between * ( ) s G t and ( ) s M t after the global optimization, a synchronized optimization algorithm is proposed in this paper. The first detected landmark  in the key frame sequence is selected as the node, then the * ( ) can be calculated as follows:


Above all, the mismatched landmark is optimized based the accumulate corrections of the minimal re-projection path.The global optimization of all landmarks can be achieved through the continuous updating and incremental optimization of the map with the proposed synchronized optimization framework.

Experiment and Analysis
In order to verify the proposed SLAM algorithm, a 4  4 m array contains 10  10 ArUco landmarks is designed in this section. The array is arranged regularly and the ID is incremented. In the experiment, the precise map (Benchmark) of the array is obtained by manual calibration. The parameters are given as follows: the landmark size is 18 cm, array's row spacing is 38 cm, the column spacing is 38.5 cm.
The hardware system uses a PS3eye camera (60 frames, 320 240 resolution) to capture real-time video, which is processed by an image processor (IntelRAtom 294i5-4300U 1.9 GHz CPU running Ubuntu14.04). The proposed algorithm is implemented by Opencv3.0, Eigen and g2o library.

Online Mapping Experiment
In order to verify the performance of the proposed algorithm, the experimental results are compared with the online mapping algorithm (Method1) proposed in [6] and the off-line mapping algorithm (Method2) proposed in [11]. The absolute center error (ACE) and the absolute trajectory error (ATE) is used to evaluate their performance. The experimental results are shown in figure 3. In figure 5, the red line is the real camera trajectory , and the blue line is the camera trajectory calculated by the each mapping algorithm. The proposed online mapping algorithm and the Method 2 algorithm have successfully built the map, while the map built by the Method1 algorithm is distorted.  Table 2 gives the detail comparison results of three algorithms. It can be seen that although the Method2 algorithm has the best mapping performance, but it is running off-line and spends 16.5 s. Although the Method1 algorithm implements online mapping, but it is easy to be disturbed by false detection. The proposed algorithm has a good balance between real-time, robust and accuracy. Although there are some accumulated errors, but it can be optimized by the proposed synchronized optimization algorithm latter.

Synchronization Optimization Experiment
In order to verify the synchronized optimization algorithm the following experiments are designed. The initial pose graph containing 42 landmarks and only 32 landmarks are recognized in the key frame sequence. The experimental results are shown in figure 6.    It can be seen from table 3 that the proposed algorithm is the only one that establishes the complete map, it not only ensures the accuracy of the map, but also guarantees less optimization time.

Simultaneous Localization and Mapping Experiment
In order to verify the overall performance of the proposed algorithm three simultaneous localization and mapping experiments are carried out. The experimental results are shown in the figure 7.  The red landmarks are the newly added landmarks with the proposed online mapping algorithm, the purple landmarks are the fixed landmarks duiring online correction and the black landmarks are the optimized and synchronized landmarks. figure 7(2) shows the mapping result of randomly positioned and orientation aligned landmark array. figure 7(3) shows the mapping result of the randomly positioned and orientated landmark array. As can be seen in figure 7, the accurate maps of three different landmark arraies are established, there is almost no absence of landmarks, and the cumulative error of the map is optimized with the proposed synchronized optimization framework. In the experiment of figure 7(3), the UWB system is used as the ground turth of the camera. The ATE of the proposed algorthim is low than 4.9 cm, which meet the location requirement of low-cost robot system such as quadrotor.

Conclusion
In this paper, a online SLAM algorithm for ArUco landmark array is proposed. The algorithm can update and correct the global map online. The accumulated error is eliminated by minimizing the reprojection error of the landmark vertex in key frame sequence. The synchronized optimization process is used to optimize the mismatched landmarks. By comparing with the existing mapping system, the experiment results verify that the proposed algorithm can realize online mapping and avoid landmark loss or map distortion while ensuring the accuracy of mapping.