Collaborative Visual Inertial SLAM for Multiple Smart Phones

There is no doubt that every VIO system can be used as a front end for agents to process new frames, as long as it is keyframe-based. The VIO module used in the proposed system is the VIO module of Vins-Mobile [22], because it is one of the best monocular VIO available at that time, and it can run stably on the ios system. The communication module is integrated into this VIO system. To be able to experiment on public datasets, such as EuRoc[23], we also add specific processing of the data, such as dedistortion, reading and flipping of image data, and alignment of measurement units.

The server map container contains all the server submaps created by the agent handler and contains all the experience gained by the agent in one task. After initializing the system, each time an agent requests to connect to the server, an agent handler is created for the agent. When at least two agent handlers are created, the server creates a server map container. A multi-map fusion optimization is carried out when the server detects the common area of two submaps, and the two submaps are fused into one map.

The agent handler manages the data that the agent sends to the server and forwards messages to the server when necessary. As shown in Fig. 1, for each agent, an agent handler module is instantiated, creating six modules that run on five separate threads, where the fifth and sixth modules are running on the same thread. It creates a communication module to communicate with the agent and a module that parses data packets into keyframe information, initializes a server submap for each agent, puts it in the server map, and establishes a server submap manager. The submap manager can detect loop inside the submap, identify the common areas of multiple submaps, run global optimization within submap or between multiple submaps.

For the communication module, a keyframe has an average of 70 map points, 700 fast corners, and the corresponding number of 256-bit descriptors. Considering the large amount of data sent by network communication at one time, the efficiency of automatic packet cutting and packet splicing is lower than that of manual packet cutting and data splicing, and the transmission frequency and density also affect the communication efficiency. Besides, there is the phenomenon of packet loss in the actual situation, and the loss of a large packet means a larger amount of retransmitted data. Therefore, we divide the packet into 4*1024 sizes artificially and set the cache area for receiving data on the server to 210*1024. The data sent by the server to the agent is almost only the pose drift of the keyframe obtained by global optimization. The amount of data for this is very small, so the data packets communicated between the server and multiple agents are mainly the map information sent by the agent to the server. This is also proved in Fig. 4 below.

The place recognition module detects the common area between a keyframe and the location in the server map container. Since each submap sent by the agent is incremental, the position overlap is detected each time with the latest frame. For each keyframe received by the server, two types of location recognition retrieval can be performed. One is place recognition with the internal map. When the place recognition module detects a pair of matching keyframes inside the agent ($KF_{old}$, $KF_{cur}$), if the map has been fused with other maps, it will trigger multi-map fusion module, otherwise, it will trigger inter-map BA module. The other is to match the location in the server map container. If the common area between agents is detected, a $SE(3)$ transformation between two matching keyframes is calculated, and more constraints are added between the two submaps, which is also the condition of the map fusion module.

When the place recognition module detects a pair of matching keyframes ($K_{c}$, $K_{m}$), which belong to the map $M_{c}$ and $M_{m}$ respectively. The alignment transformation matrix $T_{im\_ic}$ between the two keyframes is solved by the PNP algorithm ($M_{m}$ world coordinate system to $M_{c}$ world coordinate system transformation). We check $T^{Wm}_{ic\_im}\in SE(3)$ between $K_{c}$ and $K_{m}$ (the transformation matrix from $K_{m}$ to $K_{c}$ in the world coordinate system of $M_{m}$). If the yaw and 2-norm of the translation are both below the corresponding threshold, the assumption of position recognition will be accepted. If the location recognition assumption is accepted, the multi-map fusion module will be triggered. A short-term map fusion is carried out on the local window jointly defined by the keyframes around $K_{c}$ and $K_{m}$ and the map points observed by the keyframes, to optimize $T^{Wm}_{ic\_im}$. A long-term pose graph fusion optimization is performed for all the keyframes between the early position overlap keyframe and the nearest position overlap keyframe when the position overlap is accepted. The specific steps of the algorithm are as follows:

• •

  Local window. Multi-map fusion module respectively takes $M-1$ frame around the $K_{c}$ and $K_{m}$, and put them into $vKF_{c}$ and $vKF_{m}$ respectively, and puts $K_{c}$ and $K_{m}$ into the container correspondingly. Then, the 3D map point $MP_{m}$ observed in the keyframe of $vKF_{m}$ is projected onto the image coordinate system of $K_{c}$ according to the $T^{Wm}_{ic\_im}$ obtained previously. If the depth is negative or higher than the threshold, the matching of the point pair is rejected. Because of the same pose estimation accuracy, the larger the actual depth, the less reliable the point, and the worse the triangulation accuracy. The projection point in the pixel coordinate system is marked as $\rho$, then takes $\rho$ as the center of the circle and $r$ pixels as the radius to search feature points, and calculate the Hamming distance between $MP_{m}$ and the feature points. Then, the one with the smallest Hamming distance is selected as the candidate matching point. Then, the fundamental matrix is calculated by RANSAC to filter out the outer points, and the matching point pairs are respectively placed in $vMP_{c}$ and $vMP_{m}$. Then the local window makes a reverse projection to find more matching point pair. The keyframes in $vKF_{c}$ and $vKF_{m}$, and matching point pairs define a local window.

• •

  Map fusion. As shown in Fig. 2, a local BA is executed to optimize $T_{im\_ic}$. If the optimized $T_{im\_ic}$ is accepted, the two submaps are fused into one map.

• •

  Optimization of pose graph. As described in Fig. 3, if the $T_{im\_ic}$ solved in map fusion is accepted, global pose graph optimization between multiple submaps is performed. The drift corrected by the optimization results is propagated to subsequent map sequences.

To evaluate the accuracy of our proposed system, we first evaluate the absolute trajectory error (ATE) of the keyframe trajectory. The Table II compares the performance of VINS-Mobile1, VINS-Mobile2, VINS-Mobile2+Server, and VINS-Mono. VINS-Mobile1 adds the processing of reading text data, image flipping, and dedistortion on VINS-Mobile. We observe that the ATE of VIN-Mobile1 without loop optimization on MH_02 is 0.048m. After the loop optimization is done, the error becomes 0.10m. The ATE without loop optimization on MH_01 is 0.083m. After the loop optimization, it can reach 0.08m in the worst case. The loop optimization effect of VINS-Mobile1 on iphone6s is better than that on the iphone7p, even it has more computing resources. This is because the loop optimization overfitting the relative pose between the two matching frames, but this relative pose is subject to error. Based on the above analysis, we slightly modify the VINS-Mobile1 to VINS-Mobile2 by adding a filter processing which is used to avoid overfitting of loop optimization. Both VINS-Mobile1 and VINS-Mobile2 run a complete SLAM system on the mobile devices. VINS-Mobile2+Server runs VIO on the mobile devices and performs loop detection and global optimization on the server. VINS-Mono runs a complete SLAM system on the PC. Table II shows that the VINS-Mobile2+Server mode has a slightly better effect than VINS-Mono except for the MH_04 sequence. Experiment shows that the threshold value of the relative pose between two matching frames is too large, leading to the removal of the correct relative pose relationship. This threshold has not been tested much and may need to be adjusted further. Besides, the network we use is a little unstable because so many people are using it at the same time. If a better network is used, part of the experimental effect can be improved. Because during the experiment, we find that the same data is run 6 times, and the maximum error is 2cm different from the minimum error. This is because the relative pose between the two matching frames required for loop optimization is calculated in the sliding window. The delay of the network causes the matching frame to be removed from the sliding window, and the relative pose has not been calculated yet, resulting in this loop optimization not being performed. This is also the problem we need to solve later.

When the agent is running , the source editor we used, i.e. the Xcode, can monitor the occupancy rate of the system resources. Fig. 4 shows the real-time network occupancy rate of an agent, where the speed of receiving and sending data is real-time, not the average speed computed from total data volume divided by time, so the receiving data speed is 0.0KB/s, and similarly 0.4MB/s only represents the received speed of the last column in the bottom table in Fig. 4. The average bandwidth of an agent is about 0.8MB/s ($(95.4\text{Mb}+23.6\text{kb})/119\text{s}\approx 0.8\text{MB/s}$). Besides, we can see that the transmitted data is relatively uniform, which also proves that, as described in the previous section 4.2, actively cutting large data packets and controlling the transmission frequency can effectively alleviate network congestion. Since the server has been fused and optimized in the later stage, the data fed back to the agent is more low-frequency.

After analyzing the trajectory accuracy of a single agent, the accuracy of the fusion between multiple agents is evaluated. To better reflect the accuracy of the fusion, we divide each sequence of the EuRoc datasets into two parts, denoted as $A$ and $B$, and given to two agents respectively. The two agents run independently and only communicate with the server, and the server integrates the map information of the two agents. The second and third column in Table III are the ATE of sequence $A$ and sequence $B$, respectively. The fourth column is the ATE of the global map obtained by merging sequence $B$ into the world coordinate system of the A sequence. The fifth column is the ATE of the global map obtained by merging sequence $A$ into the world coordinate system of sequence $B$. The sixth column is the ATE of a map obtained after an agent runs a complete data sequence and sends it to the server for loop optimization.

Table III shows that the ATE of sequence $A$ and sequence $B$ is almost larger than that of a complete sequence. This is because the effect of map initialization has a greater impact on the ATE of the entire map, and small data sequences have less information. Moreover, $A$ and $B$ who are the main maps have a great influence on the accuracy of the final fusion. Because the proposed optimization strategy is to optimize the keyframes between the first matching frame and the last matching frame of the main map, while the other maps are optimized from the first frame of the map to the last matching frame. The drift of the optimization correction propagates to the subsequent map sequence, but the previous map sequence cannot be corrected unless the previous map sequence has a new loop constraint. Column 2 and column 3 in Table III show that mapping only through VIO tracking is bad. It is found in the experiment that the filtering condition is too strong to ensure the accuracy of fusion, which leads to the common area detected in the previous sequence of the main map being regarded as not a correct hypothesis and cannot participate in the optimization of multi-map fusion.

The last experiment aims to analyze the ability of agents to share and reuse information in a collaborative environment. For this reason, we have experimented in single-agent mode, double-agent mode and VINS-Mono multi-session mode. Their ATE reported in Table IV are calculated by aligning the optimized global trajectory with the ground truth. For each experiment, in the case of multi-agent and VINS-Mono multi-session, we only align and evaluate the results of the second sequence. For the single-agent case, we only run the second sequence. Because there are very few overlapping regions in the MH_02 and MH_03, the proposed system only accepts two overlaps, resulting in the fusion error of the two submaps that can not be optimized. For the MH_03 sequence, the ATE after the fusion optimization is higher than the ATE performing loop optimization only, which exists in the proposed system and VINS-Mono. The experiment shows that the other data sequences in Table IV have relatively more overlapping areas, and their mapping accuracy is higher than that in the single-agent mode. This shows that the proposed fusion optimization strategy is feasible, even if the relative pose of the two matched frames is inaccurate. But if there are fewer common areas between two submaps, the error will propagate to the subsequent map sequence and cannot be corrected. Improving the accuracy of the relative pose between two frames is an important problem we will solve in the future. Table IV also shows that the fusion accuracy of the proposed method has achieved similar results to VINS-Mono, and the benefits of sharing information obtained by multiple agents to improve accuracy during collaboration.