3D Visual SLAM Based on Multiple Iterative Closest Point

Algorithm 1

(1)    for current frame   do

(2)      motion data

(3)     previous frame with

(4)     ICP(current frame , previous frame )

(5)     if   ICP succeeds  then

(6)       Update  

(7)     else

(8)       find nearest frame    with in map  M

(9)       ICP(current frame , frame )

(10)       if   ICP succeeds  then

(11)         Updated  

(12)   else

(13)        Updated failed, discard the frame

(14)   end if

(15)   end if

(16)   if   Update succeeds  then

(17)     if    then

(18)      add frame to  M

(19)     end if

(20)   end if

(21) end