Table - PMC

Skip to main content

View full-text article in PMC

. 2021 Jun 8;21(12):3955. doi: 10.3390/s21123955

Algorithm 1: Loop closure algorithm.

Input: $T_{m}, P_{m}$ from the sliding window

Output:

T_{I C P}

1:
if $(T_{m}, P_{m}) .$ isKeyframe() then
2:
if $D_{T} \neq ϕ$ or $D_{P} \neq ϕ$ then
3:
$T_{m}^{^{'}} \leftarrow$ KDtree.RadiusSearch $(T_{m}, D_{T}, r);$
4:
$M \leftarrow$ registerPointCloud $(D_{T}, D_{P});$
5:
if $T_{m}^{^{'}} \neq 0$ then
6:
$T_{I C P} \leftarrow$ ComputeICP $(P_{m}, M, T_{m});$
7:
end if
8:
end if
9:
$T_{m}^{^{'}} \leftarrow 0$
10:
$D_{T} = D_{T} \cup T_{m}$
11:
$D_{P} = D_{P} \cup P_{m}$
12:
end if