Intensity-Assisted ICP for Fast Registration of 2D-LIDAR

. 2019 May 8;19(9):2124. doi: 10.3390/s19092124

Algorithm 1: Intensity-Assisted ICP

Input: Source frame <P₁, I₁> and target frame <P₂, I₂>, the search criteria C_θ, C_x, C_y, the search step S_θ, S_x, S_y
Output: Convergence transformation T_final
get sampled point cloud from P₁, P₂ with Section 3.1
get C_θ, C_x, C_y, S_θ, S_x, S_y with Algorithm 2
n = 1
for θ = −C_θ: S_θ: C_θ do
for x = −C_x: S_x_: C_x do
for y = −C_y: S_y_: C_y do
get the SCORE^(I) and SCORE^(G) with u_n produced by θ, x, y
n++
end for
end for
end for
get u_optimal with Section 3.2
T₀ = u_optimal
Anderson acceleration begin with T₀ between P₁, P₂
when convergence criteria is true, return T_final