A Dense Mapping Algorithm Based on Spatiotemporal Consistency

View full-text article in PMC

. 2023 Feb 7;23(4):1876. doi: 10.3390/s23041876

Algorithm1. Local Map Extraction.

Input:
j is the index of the current frame. $T_{w F_{j}}$ is the pose of the current frame. $p o s e D a t a b a s e$ is the pose database that stores the poses of each frame and their surfels. $f a r_d i s t$ is the maximum mapping distance.
Output:
$l o c a l I n d e x e s$ is a vector of the local frame indexes. $l o c a l S u r f e l s$ is a vector of the local surfels.
1:
$l o c a l I n d e x e s .$ CLEAR()
2:
$l o c a l S u r f e l s .$ CLEAR()
3:
for each $F_{i} \in p o s e D a t a b a s e$ do
4:
$f l a g \leftarrow f a l s e$
5:
$T_{F_{j} F_{i}}$ ← transform( $T_{w F_{i}}$ , $T_{w F_{j}}$ )
6:
$[u, v]$ ← project( $T_{F_{j} F_{i}}$ )
7:
if isValidRange( $[u, v]$ ) && $T_{F_{j} F_{i}} ∣_{z}$ ≤ $k \cdot f a r_d i s t$ then
8:
$f l a g \leftarrow t r u e$
9:
end if
10:
$T_{F_{i} F_{j}}$ ← transform( $T_{w F_{j}}$ , $T_{w F_{i}}$ )
11:
$[u, v]$ ← project( $T_{F_{i} F_{j}}$ )
12:
if isValidRange( $[u, v]$ ) && $T_{F_{i} F_{j}} ∣_{z}$ ≤ $k \cdot f a r_d i s t$ then
13:
$f l a g \leftarrow t r u e$
14:
end if
15:
if distance( $T_{w F_{i}}$ , $T_{w F_{j}}$ ) ≤ $k \cdot f a r_d i s t$ && angle( $T_{w F_{i}}$ , $T_{w F_{j}}$ ) ≤ $θ_{t h}$ then
16:
$f l a g \leftarrow t r u e$
17:
end if
18:
if $f l a g$ then
19:
for $t \leftarrow - n, - n + 1 . . . n - 1, n$ do
20:
$l o c a l I n d e x e s$ .PUSH( $i + t$ )
21:
end for
22:
end if
23:
end for
24:
for each $i \in l o c a l I n d e x e s$ do
25:
$l o c a l S u r f e l .$ INSERT( $p o s e D a t a b a s e$ [i].surfels)
26:
end for