A Fast Map Merging Algorithm in the Field of Multirobot SLAM

Algorithm 3

Random walk optimization.

Input: the grid map 𝑚₁, 𝑚₂, initial relative pose estimation 𝑡_start

Output: the relative pose 𝑡 between map 𝑚₁ and 𝑚₂

1  𝑘=0;  𝑡_𝑘=𝑡_start;

2  Σ₀=Σ_init;  𝜇₀=𝜇_init;

3  𝑐₀=𝜓(𝑚₁,𝑇_𝑡start(𝑚₂));

4 while 𝑘<𝑛𝑢𝑚𝑠𝑡𝑒𝑝𝑠  do

5   𝜈_𝑘∼𝑁(𝑢_𝑘,Σ_𝑘);

6   𝑠=𝑡_𝑘+𝜈_𝑘;

7   𝑐_𝑠=𝜓(𝑚₁,𝑇_𝑠(𝑚₂));

8   if  𝑐_𝑠<𝑐_𝑘  or  RS(𝑡_𝑘,𝑠)=𝑠  then

9    𝑘=𝑘+1; 𝑡_𝑘=𝑠; 𝑐_𝑘=𝑐_𝑠;

10   Σ_𝑘+1=(𝑡_𝑘,𝑡_𝑘−1,…,𝑡_𝑘−𝑀);

11   𝜇_𝑘+1=(𝑡_𝑘,𝑡_𝑘−1,…,𝑡_𝑘−𝑀);

12   end

13 end

14 return  𝑠;