# ROSEFusion: Random Optimization for Online Dense Reconstruction under Fast
Camera Motion

Jiazhao Zhang  National University of Defense TechnologyChina ,  Chenyang Zhu
National University of Defense TechnologyChina ,  Lintao Zheng  National
University of Defense TechnologyChina and  Kai Xu  National University of
Defense TechnologyChina

###### Abstract.

Online reconstruction based on RGB-D sequences has thus far been restrained to
relatively slow camera motions (¡1m/s). Under very fast camera motion (e.g.,
3m/s), the reconstruction can easily crumble even for the state-of-the-art
methods. Fast motion brings two challenges to depth fusion: 1) the high
nonlinearity of camera pose optimization due to large inter-frame rotations
and 2) the lack of reliably trackable features due to motion blur. We propose
to tackle the difficulties of fast-motion camera tracking in the absence of
inertial measurements using random optimization, in particular, the Particle
Filter Optimization (PFO). To surmount the computation-intensive particle
sampling and update in standard PFO, we propose to accelerate the randomized
search via updating a particle swarm template (PST). PST is a set of particles
pre-sampled uniformly within the unit sphere in the 6D space of camera pose.
Through moving and rescaling the pre-sampled PST guided by swarm intelligence,
our method is able to drive tens of thousands of particles to locate and cover
a good local optimum extremely fast and robustly. The particles, representing
candidate poses, are evaluated with a fitness function defined based on depth-
model conformance. Therefore, our method, being depth-only and correspondence-
free, mitigates the motion blur impediment as (ToF-based) depths are often
resilient to motion blur. Thanks to the efficient template-based particle set
evolution and the effective fitness function, our method attains good quality
pose tracking under fast camera motion (up to 4m/s) in a realtime framerate
without including loop closure or global pose optimization. Through extensive
evaluations on public datasets of RGB-D sequences, especially on a newly
proposed benchmark of fast camera motion, we demonstrate the significant
advantage of our method over the state of the arts.

RGB-D reconstruction, online dense reconstruction, random optimization, fast-
motion camera tracking

††copyright: acmcopyright††journal: TOG††journalyear: 2021††journalvolume:
40††journalnumber: 4††article: 56††publicationmonth: 8††doi:
10.1145/3450626.3459676††ccs: Computing methodologies Shape modeling
\begin{overpic}[width=433.62pt,tics=5]{fig/img/teaser.pdf} \end{overpic}
Figure 1.  We introduce ROSEFusion, a depth-only online dense reconstruction
which is stable and robust to highly fast camera motion. Built upon the
volumetric depth fusion framework, our method solves the highly nonlinear
optimization problem of fast-motion camera tracking using random optimization.
In this example, the depth camera moves at a speed of $2$m/s in average and up
to $3.6$m/s, leading to severe motion blur in RGB images. This sequence would
break most state-of-the-art online reconstruction methods. Thanks to our novel
particle filter optimization with swarm intelligence, our method is able to
fuse the depth maps (resilient to motion blur) accurately, attaining a
satisfying reconstruction quality. See the planar walls and the correct
overall layout; the imperfect local geometry was mainly due to incomplete
depth scanning. The tracked camera trajectory is visualized and color-coded
with camera speed.

##  1\. Introduction

With the proliferation of commodity RGB-D cameras, the recent decade has
witnessed a booming of online RGB-D reconstruction techniques. Since the
seminal work of KinectFusion (Newcombe et al., 2011a), a huge body of works
have been proposed based either on volumetric depth fusion (Nießner et al.,
2013; Chen et al., 2013) or point-based fusion (Keller et al., 2013; Whelan et
al., 2015). Despite the significantly advanced frontier on reconstruction
quality and scalability, the prior systems have hereunto been restrained to
_slow camera motions_ , typically less than 1m/s. Under faster camera motions,
the RGB-D reconstruction could easily collapse due to drastically increased
difficulties in tracking and mapping. This has greatly limited the practical
use of these techniques especially in autonomous scanning and reconstruction
by, e.g., UAVs.

Fast camera motion brings two main challenges to RGB-D fusion. _First_ , fast
motions lead to large rotations which make camera pose optimization highly
nonlinear and prune to local optima for gradient descent methods (Schmidt and
Niemann, 2001). _Second_ , fast camera movement causes severe motion blur in
RGB images, especially under dark lighting conditions and in the case of
indoor-level camera-to-scene distance. This makes it intractable for robust
photometric tracking which is crucial to many SLAM (Mur-Artal et al., 2015)
and dense RGB-D reconstruction methods (Dai et al., 2017).

We propose, ROSEFusion, the first end-to-end solution to online dense RGB-D
reconstruction under _fast camera motions_ without inertial measurement. Our
method relies solely on depth information for robust camera tracking and
accurate volumetric fusion, observing that depth sensors (especially time-of-
flight (ToF)) are usually more resilient to motion blur. When the camera moves
fast, depth map defect often exhibits as overshoot or undershoot in the
transition between foreground and background (Hansard et al., 2012) but not as
full-frame pixel mixing/blurring as in RGB images (see Figure 1). The
detection and removal of the false depth signals appearing around occlusion
edges can be easily realized with hardware (Lee, 2014) and has usually been
done by most commercial ToF cameras.

To tackle the challenges in fast-motion camera tracking mentioned above, we
propose two key designs. _Firstly_ , we propose to solve the highly nonlinear
optimization of large inter-frame camera transformation by _random
optimization_ , in particular, the Particle Filter Optimization (PFO) (Liu et
al., 2016). To surmount the computationally intensive particle sampling and
update in standard PFO, we propose to accelerate the randomized search via
updating a _particle swarm template (PST)_ , a set of particles which are
_pre-sampled_ uniformly within the unit sphere in the 6D space of camera pose.
Specifically, the PST is _moved and rescaled_ progressively, dictated by the
so-far best solution found in the particle set which maximizes an observation
likelihood. Through evolving the pre-sampled PST, our method is able to drive
tens of thousands of particles to locate and cover a good local optimum
extremely fast and robustly.

_Secondly_ , the sampled particles, representing candidate poses, are
evaluated with a depth-based _discriminant fitness/likelihood_ function. The
fitness of a particle is measured as depth-model conformance through
integrating the truncated signed distance field (TSDF) values over the depth
map transformed by the corresponding camera pose. This can be efficiently
evaluated based on TSDF occupancy queries and requires no frame-to-frame
feature correspondence which is difficult under fast motion. Thanks to the
efficient template-based particle maintenance and the effective observation
likelihood, our method attains good quality pose tracking under fast camera
motion (up to $4$m/s) in a realtime framerate ($30$Hz) without depth map
filtering, frame dropping, or global pose optimization.

We have evaluated our method on several public benchmarks. On ordinary speed
sequences, our method achieves comparable accuracy of camera tracking against
the state-of-the-art methods, without needing post-processing such as loop
closure detection or global pose optimization. On the ETH3D benchmark (Schops
et al., 2019), our method successfully reconstructs those challenging
sequences with fast camera motion (i.e., shaking cameras) on which all
previous methods failed. We also contribute a new benchmark named FastCaMo
which encompasses both synthetic and real captured sequences with fast camera
motion ($3\sim 4$ times faster than the existing datasets). Extensive
quantitative and qualitative evaluations demonstrate the significant advantage
of our method on fast-motion camera tracking and dense reconstruction.

To sum up, the contributions of this work are:

  * •

Problem: We study the problem of online dense reconstruction under fast camera
motion without using an IMU and propose the first solution to it based on
random optimization.

  * •

Optimization: We propose a novel particle filter optimization which achieves
robustness and effectiveness via evolving a pre-sampled particle swarm
template. Through detailed comparisons to the classical particle filter
optimization and particle swarm optimization, we show that our method achieves
a much better balance between exploration and exploitation.

  * •

Benchmark: We propose the first dataset of fast-camera-motion RGB-D sequences,
along with ground-truth trajectories and reconstructions for synthetic and
real captured data, respectively.

  * •

System: We have implemented an end-to-end system of online RGB-D dense
reconstruction which realizes robust and quality realtime reconstruction under
fast camera moving speed up to $4$m/s.

##  2\. Related work

#### Online RGB-D reconstruction.

There is a large body of literature on offline and online RGB-D
reconstruction; let us review only those highly related ones. KinectFusion
(Newcombe et al., 2011a; Izadi et al., 2011) is one of the first to realize a
real-time volumetric fusion framework of Curless and Levoy (1996). In order to
handle larger environments, spatial hierarchies (Chen et al., 2013), and
hashing schemes (Nießner et al., 2013; Kahler et al., 2015) have been
proposed. Another line of research adopt point-based representation (Whelan et
al., 2012; Keller et al., 2013; Henry et al., 2014; Whelan et al., 2015).

Real-time per-frame camera pose estimation is a core problem in the
Simultaneous Localization and Mapping (SLAM) literature. Several real-time
monocular RGB methods (e.g., (Klein and Murray, 2007; Engel et al., 2013;
Forster et al., 2014; Engel et al., 2014)) typically rely on either pose-graph
optimization (Kuemmerle et al., 2011) or bundle adjustment (Triggs et al.,
1999). In the case of dense reconstruction, MonoFusion (Pradeep et al., 2013)
integrates sparse SLAM bundle adjustment with dense volumetric fusion. DTAM
(Newcombe et al., 2011b) estimates camera poses directly from the
reconstructed dense 3D model based on frame-to-model tracking. Based on GPU
optimization techniques, several methods (Schops et al., 2019; Dai et al.,
2017) realizes real-time global pose alignment.

Common to most existing state-of-the-arts is the reliance on photometric-
error-based objective and gradient-descent-based optimization (e.g. (Dai et
al., 2017)). Bylow et al. (2013) realize a feature-free camera tracking via
defining an objective function of pose optimization based on depth-to-TSDF
agreement. Our fitness function is defined similarly. However, they still
employ the gradient descent method in their optimization. To the best of our
knowledge, our work is the first that utilizes random optimization for camera
pose estimation, at least in the context of online dense reconstruction.

#### Camera tracking with inertial measurements.

Visual-inertial solution to camera tracking for SLAM, odometry or online
reconstruction is an active field of research (Scaramuzza and Zhang, 2019) and
has been successfully deployed in practice. IMUs provide acceleration data at
a high frequency and can be used to predict inter-frame motions serving as
good initialization for gradient-descent-based optimization (Forster et al.,
2016). The gyroscope sensor of IMUs (especially built-in IMUs in commodity
RGB-D cameras) is more effective for measuring changes in orientation than
estimating translations. Many researchers found the translation error too
large to be useful in tracking, either used as pose initialization (Nießner et
al., 2014) or for joint optimization (Laidlow et al., 2017).

To our knowledge, most VIO works are mainly designed for large-scale
environments with decimeter-level tracking accuracy (Mourikis and Roumeliotis,
2007; Leutenegger et al., 2015). The accuracy is primarily determined by
visual tracking which is prone to motion blur, although the latter is usually
not a significant issue for outdoor environments due to large camera-to-scene
distance (Cui et al., 2019; Pollefeys et al., 2008; Engel et al., 2015). These
existing methods, however, usually cannot support a centimeter-level fast-
motion camera tracking in the case of indoor-level camera-to-scene distance.
Saurer et al. (2016) additionally considers the rolling shutter effect under
fast camera motion. Another line of research uses event camera for fast motion
camera tracking (Gallego et al., 2019).

#### Pose tracking based on particle filter.

Let us differentiate our work from the large literature of camera/object pose
tracking based on particle filter (PF), and more specifically, Rao-
Blackwellized particle filter (RBPF) (Andrieu and Doucet, 2002). There has
been extensive research on particle filter based SLAM (Grisetti et al., 2007;
Gil et al., 2010; Choi and Christensen, 2012) and object pose tracking (Deng
et al., 2019; Nieto et al., 2016; Arnaud and Mémin, 2005).

The key difference of the particle filter utility between our method and the
line of works above lies in the objective. Taking the camera tracking in SLAM
as an example, those previous works (e.g. (Grisettiyz et al., 2005)) optimize
for sequential state (pose) estimation throughout _a sequence of frames_ via
maximizing observation likelihood. On the other hand, our work optimizes the
camera pose of _a single (current) frame_. In short, the sequential importance
sampling is across different frames in (Grisettiyz et al., 2005) and across
increasing iteration steps for the pose of a single frame in our work. Figure
3 contrasts the two different problems with their underlying probabilistic
graphical models.

#### Particle Filter Optimization (PFO) and Particle Swarm Optimization (PSO).

The sequential Monte-Carlo method of particle filter has been employed in
solving global optimization problems (Liu et al., 2016; Zhang et al., 2017).
The basic idea of PFO algorithms is to transform the objective function into a
target PDF and then perform sequential importance sampling to simulate the
target probability density function (PDF). The hope is that the optimum of the
objective function can be covered by sampled particles. PSO is a well studied
heuristic optimization technique inspired by the social behavior in nature
(Shi and Eberhart, 1999). In PSO, a set of particles are generated randomly
and moved according to their own experience and the experience of the swarm
(swarm intelligence). PSO suffers the premature convergence making it easily
get stuck in local optima.

The core of PFO is how to design the system dynamic function which drives the
set of particles to move toward good local optima. In a recent attempt, PSO
update is used as system dynamic of the state space model (Ji et al., 2008).
However, sampling and updating particles in PFO is still time-consuming. This
explains why PFO has not been widely adopted in realtime applications. Our
method improves PFO by moving and rescaling a particle swarm template thus
avoiding particle sampling and resampling in standard PF.

##  3\. Method

#### Overview.

The input to online reconstruction is an RGB-D sequence
$\\{I_{c}^{t},I_{d}^{t}\\}_{t=0:T}$ ($I_{c}$ and $I_{d}$ are RGB and depth
images, respectively) captured by an RGB-D camera and the output is a surface
reconstruction of the scene being captured, $\mathcal{S}$, and a trajectory of
6DoF camera poses, $\\{[\mathbf{R}^{t}|\mathbf{t}^{t}]\\}_{t=0:T}$
($\mathbf{R}\in SO(3)$ and $\mathbf{t}\in\mathbb{R}^{3}$ are 3D rotation and
3D translation in the global coordinate system, respectively). We build our
method upon the volumetric depth fusion framework (Curless and Levoy, 1996;
Izadi et al., 2011) which is the de facto method for high-quality online dense
reconstruction. The key problem of online reconstruction is the estimation of
6D camera pose for each frame. Based on the per-frame camera pose, the
corresponding depth map can be fused into the 3D volume incrementally. To
handle fast camera motion, we rely only on depth input for camera pose
tracking, which is our key contribution. Of course, our method could admit RGB
image input for camera tracking and relocalization when the camera motion is
slow and image features are robustly detected.

This section focuses on the key problem — depth-based per-frame camera pose
optimization. After describing the problem formulation (Section 3.1), we
introduce our random optimization framework (Section 3.2). We then introduce
how to improve the optimization by replacing the sequential importance
sampling with particle swarm template evolution (Section 3.3).

###  3.1. Formulation of Per-Frame Pose Optimization

Given the depth image of the current frame $I_{d}^{t}$ and the so-far
constructed TSDF $\psi:\mathbb{R}^{3}\rightarrow\mathbb{R}$, our task is to
compute the 6-DoF camera pose of $I_{d}^{t}$ in the global coordinate system:
$[\mathbf{R}^{t}|\mathbf{t}^{t}]\in SE(3)$. Hereafter, we would omit the frame
index $t$ and use $[\mathbf{R}|\mathbf{t}]$ to represent the global camera
pose of the current frame to be optimized.

\begin{overpic}[width=433.62pt]{fig/img/tsdf_min.pdf}
\put(28.0,33.5){\small(a)} \put(75.0,33.5){\small(b)} \end{overpic} Figure 2.
Illustration of per-frame camera pose optimization by minimizing the frame-to-
model (depth-to-TSDF) error function in Eq. (4). (a): TSDF-based depth fusion
and TSDF embedding $\mathbf{R}\mathbf{x}+\mathbf{t}$ of an unprojected 3D
point $\mathbf{x}$ of $I_{d}$ under a camera pose $(\mathbf{R},\mathbf{t})$.
The grid plot shows squared TSDF values (blue is small and red is large). (b):
For frame $t$, the depth map is first embedded into the TSDF volume with the
candidate camera poses $(\mathbf{s}^{t,(i)})_{i=1:N}$. The optimal pose is the
one that minimizes the sum of retrieved TSDF values by all unprojected 3D
points. In (b), $\mathbf{s}^{t,(i+1)}$ is a better solution since the summed
TSDF values over the blue voxels is lower than that of $\mathbf{s}^{t,(i)}$
over the orange voxels.

We base our approach on the depth-fusion-based pose estimation (Bylow et al.,
2013) which defines a frame-to-model error metric to evaluate the goodness of
a camera pose $[\mathbf{R}|\mathbf{t}]$ by measuring how well $I_{d}$ “fits
into” the TSDF under $[\mathbf{R}|\mathbf{t}]$. For each pixel $(i,j)$ of
$I_{d}$, suppose its depth is $z=I_{d}^{t}(i,j)$ based on which we can obtain
its corresponding 3D point $\mathbf{x}_{ij}$ in the camera coordinate system
of the current frame. We can then transform this point to the global
coordinate system:

(1) $\mathbf{x}_{ij}^{G}=\mathbf{R}\mathbf{x}_{ij}+\mathbf{t}.$

We use these unprojected 3D points to query the TSDF (defined in the global
coordinate system) and obtain a point-to-surface distance directly. If the
camera pose is correct, it is expected that the point-to-surface distances of
every unprojected 3D point should be zero. We therefore seek for the camera
pose $[\mathbf{R}|\mathbf{t}]$ such that every unprojected 3D point from the
depth image lies as close as possible to the zero-crossing surface of the
TSDF, i.e., $\tilde{\mathcal{S}}=\\{\mathbf{x}|\psi(\mathbf{x})=0\\}$. See
Figure 2 for an illustration.

Assuming that the depth measurements of the camera contain Gaussian noise and
that all pixels are independent and identically distributed, the likelihood of
observing a depth image $I_{d}$ from camera pose $[\mathbf{R}|\mathbf{t}]$ is

(2)
$p(I_{d}|\mathbf{R},\mathbf{t})\varpropto\prod_{i,j}\exp\left(-\psi(\mathbf{R}\mathbf{x}_{ij}+\mathbf{t})^{2}\right).$

Therefore, our goal is to find the optimal camera pose
$[\mathbf{R}^{\ast},\mathbf{t}^{\ast}]$ that maximizes the likelihood

(3)
$(\mathbf{R}^{\ast},\mathbf{t}^{\ast})=\arg\max_{\mathbf{R},\mathbf{t}}{p(I_{d}|\mathbf{R},\mathbf{t})}.$

In (Bylow et al., 2013), the maximum likelihood estimation is performed by
minimizing the following error function by taking the negative logarithm of
the likelihood, i.e.,

(4)
$(\mathbf{R}^{\ast},\mathbf{t}^{\ast})=\arg\min_{\mathbf{R},\mathbf{t}}\sum_{(i,j)\in
I_{d}}{\psi(\mathbf{R}\mathbf{x}_{ij}+\mathbf{t})^{2}}.$

Directly optimizing Eq. (4) using gradient descent methods (e.g. Gauss-Newton
used in (Bylow et al., 2013)) is difficult in the case of fast motion due to
the high nonlinearity caused by large rotations. Moreover, the gradient can be
undefined when the depth map is out of the valid scope of the TSDF under large
camera transformation. We therefore resort to random optimization based on
particle filter which leads to simple, efficient and robust camera tracking
under fast motion. Next, after a brief review of particle filter, we introduce
our random optimization solution to pose optimization based on particle filter
optimization.

###  3.2. Particle Filter Pose Optimization

#### Particle filter

(PF) is a class of importance sampling and resampling techniques designed to
simulate from a sequence of probability distributions for sequential inference
problems (Gordon et al., 1993). It has gained significant success in non-
Gaussian and non-linear state estimation problems.

Given a state space model,

(5) $\displaystyle s_{k}=f(s_{k-1})+w_{k},$ (6) $\displaystyle
o_{k}=g(s_{k})+v_{k},$

where $f(\cdot)$ is the system dynamic, $g(\cdot)$ is the observation
function, and $w_{k}$ and $v_{k}$ are the system noise and observation noise,
respectively. Note that $k$ indicates the time step in sequential sampling; it
has nothing to do with the frame index in the context of our work. We are
interested in estimating the posterior distribution $p(s_{k}|o_{1:k})$, where
$o_{1:k}$ denotes the observations obtained until step $k$. The posterior can
be computed in a recursive manner:

(7) $\displaystyle p(s_{k}|o_{1:k})\varpropto
p(o_{k}|s_{k})p(s_{k}|o_{1:k-1}),$ (8) $\displaystyle
p(s_{k}|o_{1:k-1})\varpropto\int{p(s_{k}|s_{k-1})p(s_{k-1}|o_{1:k-1})dx_{k-1}},$

where $p(s_{k-1}|o_{1:k-1})$ is the posterior at step $k-1$ and
$p(o_{k}|s_{k})$ the observation likelihood. To resolve the intractable
integration in Eq. (8), PF approximates the posterior with a set of weighted
Monte Carlo samples or particles $\\{s^{(i)}_{k},w^{(i)}_{k}\\}_{i=1:N}$

(9)
$p(s_{k}|o_{1:k})\thickapprox\sum_{i=1}^{N}w^{(i)}_{k}\delta\left(s_{k}-s^{(i)}_{k}\right),$

where $\delta$ is the Dirac delta function.

PF proceeds as follows. In each step, a set of particles
$\\{s^{(i)}_{k}\\}_{i=1:N}$ is generated from a proposal function:

(10) $s^{(i)}_{k}\thicksim q(s_{k}|s^{(i)}_{k-1}),\quad i=\\{1,\ldots,N\\}.$

Here, the proposal function $q(s_{k}|s_{k-1})$ usually takes some system
dynamic prior. The importance weights of the particles are updated by
multiplying the likelihood:

(11) $w^{(i)}_{k}=w^{(i)}_{k-1}p(o_{k}|s^{(i)}_{k}),\quad i=\\{1,\ldots,N\\}.$

To mitigate the particle depletion issue, a resampling step is invoked which
resamples particles according to the updated importance weights. After
resampling, regions in state space with higher importance weight will be
populated with more particles. The weights of the resampled particles are then
reset to $\frac{1}{N}$.

\begin{overpic}[width=433.62pt]{fig/img/pgm.pdf} \end{overpic} Figure 3.  The
probabilistic graphical models of online reconstruction (left) and per-frame
pose optimization (right). Left: The online reconstruction can be represented
by a hidden Markov model (HMM) (Thrun, 2002) where the per-frame camera poses
$(\mathbf{s}^{t})_{t=0:T}$, the inter-frame camera motions
$(\mathbf{u}_{t})_{t=0:T}$ and the TSDF $\psi$ are latent variables, and
$(I_{d}^{t})_{t=0:T}$ observed ones. Right: The optimization of
$\mathbf{s}^{t}$ (camera pose for frame $t$) starts from the initial state
$\mathbf{s}^{t-1}$ and proceeds until the optimal solution
$\mathbf{s}^{t}_{\ast}$ is found. Here, we maximize the objective $g^{t}$
while assuming both the observation $I_{d}^{t}$ and the TSDF $\psi$ (after
frame $t-1$ is fused) are known. The latent variables enclosed by the rounded
rectangles are to be estimated.

#### Particle filter optimization

(PFO) is a recently proposed random optimization method (Liu et al., 2016).
The basic idea is to represent the objective function as a target PDF and then
leverage sequential importance sampling to simulate the target PDF. The goal
is to cover the optimum of the objective function with samples, i.e.,
optimizers. Given a general minimization problem

$\min_{s\in S}g(s),$

where $S\subset\mathbb{R}^{d}$ is the non-empty solution space and $g$ a real-
valued objective function bounded on $S$. In PFO, the solutions $s$ are
treated as states and the objective $g(s)$ as the observation function. PFO
performs a sequential sampling of the target posterior distributions
$p(s_{k}|g_{1:k})$ until the optimum state/solution $s^{\ast}$ is reached.

In each step of PFO, a set of particles $\\{s^{(i)}_{k}\\}_{i=1:N}$ are
sampled from the proposal PDF $q(s_{k}|s_{k-1})$. A common choice of proposal
distribution is symmetric, e.g.,

(12) $q(s_{k}|s^{(i)}_{k-1})=\mathcal{N}(s^{(i)}_{k-1},\Sigma),$

where $\mathcal{N}(s^{(i)}_{k},\Sigma)$ is a normal distribution with mean
$s^{(i)}_{k}$ and covariance matrix $\Sigma$. According to Eq. (11), particle
weights are updated by multiplying the likelihood function defined as follows:

(13) $p(g|s_{k})=\exp\left(-\frac{g(s_{k})-g(s^{\ast})}{\tau}\right),$

where $\tau$ is a temperature in the Boltzmann distribution (we set $\tau=1$
by default). Based on the updated weights, particles are resampled and their
weights are reset to be uniform.

#### PFO for camera pose optimization.

In our problem setting, we treat the camera pose
$\mathbf{s}=(\mathbf{R},\mathbf{t})$ as state. Our goal is the minimization
problem stated in Eq. (4). The likelihood function corresponding to the
objective in Eq. (4) is then defined as:

(14) $p(g|\mathbf{s},I_{d})=\exp\left(-\frac{1}{\tau}\sum_{(i,j)\in
I_{d}}{\psi(\mathbf{R}\mathbf{x}_{ij}+\mathbf{t})^{2}}\right),$

given that a lower bound estimation of $g(\mathbf{s}^{\ast})$ is $0$. See the
probabilistic graphical model of PFO-based per-frame camera pose optimization
in Figure 3 and contrast it with that of online reconstruction. The former is
a subproblem of the latter.

\begin{overpic}[width=433.62pt]{fig/img/pst_evolve.pdf} \end{overpic} Figure
4.  Per-frame pose optimization by PFO with evolving PST. Starting with a PST,
$\Omega$, sampled from the unit sphere in the parametric space of 6D camera
pose (illustrated with 2D here), we optimize the camera pose for each frame
through evolving the PST. For each frame $t$, a sequential search is performed
by moving and rescaling the PST, leading to a sequence of interim solutions
$(\mathbf{s}_{k}^{\ast})_{k=1:K}$, until a sufficiently good solution (camera
pose), $\mathbf{s}^{t}_{\ast}$, is covered by the PST particles. This figure
also illustrates PST initialization for a given frame. For example, the PST of
frame $t+1$ is initialized at $\mathbf{c}_{0}^{t+1}=\mathbf{s}^{t}_{\ast}$,
with the axis lengths of the PST after the first iteration of frame $t$, i.e.,
$\mathbf{r}_{0}^{t+1}=\mathbf{r}_{1}^{t}$; see the PSTs highlighted with red
boundary.

A critical issue now is how to select a good proposal distribution
$q(\mathbf{s}_{k}|\mathbf{s}^{(i)}_{k-1})$ to enable an efficient exploration
of the solution space. A naive system dynamic such as the normal distribution
in Eq. (12) cannot be very efficient since it does not exploit the information
from the previous step. A recent trend on improving sample efficiency of
particle filter is to improve the system dynamic with the _swarm intelligence_
of all particles (Ji et al., 2008). However, sampling and maintaining a
particle swarm is computationally expensive. We propose to replace the
sequential sampling of PFO by moving and rescaling a pre-sampled particle
swarm template.

###  3.3. PFO with Particle Swarm Template

Let us first define the solution/state as the camera pose
$\mathbf{s}=(\mathbf{R},\mathbf{t})\doteq(q_{x},q_{y},q_{z},x,y,z)$ with
$q_{x}$, $q_{y}$ and $q_{z}$ being the imaginary part of the rotation
quaternion and $\mathbf{t}=(x,y,z)^{T}$. In our PFO, instead of sampling the
particles on-the-fly throughout the optimization, we opt to _pre-sample_ a
fixed number of particles uniformly within the unit sphere in the 6D state
space. The pre-sampled particle set is referred to as Particle Swarm Template
(PST), denoted by $\Omega$. Let us represent the PST by a center position
$\mathbf{c}$ and a vector of axis lengths (each for one of the six dimensions)
$\mathbf{r}=(r_{d})_{d=1:6}$. Initially, PST is sampled from the unit sphere
hence $\mathbf{c}=\mathbf{0}\in\mathbb{R}^{6}$ and
$\mathbf{r}=\mathbf{1}\in\mathbb{R}^{6}$. It is then moved and rescaled into
ellipsoids throughout the optimization steps to gradually cover the optimal
solution.

Algorithm 1 describes our PFO-based per-frame pose optimization with PST. The
algorithm evolves a pre-sampled PST with moving and rescaling through
optimization steps. The process repeats until a satisfactory solution
$\mathbf{s}^{\ast}$ is obtained or the maximum search step is reached. See
Section 4 for the details on PST initialization and termination criteria.
Figure 4 illustrates the optimization process.

Input : Depth map: $I_{d}$; Pre-sampled PST: $\Omega$.

Output : A local optimum: $\mathbf{s}^{\ast}$.

$\Omega_{0},\mathbf{c}_{0},\mathbf{r}_{0}\leftarrow$ Initialize(_$\Omega$_);

// see details in Section 4

1 $\mathbf{s}^{\ast}_{0}\leftarrow\mathbf{c}_{0}$;

2 $k\leftarrow 1$;

3 repeat

4 $\Omega_{k}^{a}\leftarrow\varnothing$;

5 foreach  _$\mathbf{s}_{k}^{(i)}\in\Omega_{k}$_ do // collect APS

$\rho(\mathbf{s}_{k}^{(i)})\leftarrow p(g|\mathbf{s}_{k}^{(i)},I_{d})$;

// evaluate fitness with Eq. (14)

6 if  _$\rho(\mathbf{s}_{k}^{(i)}) >\rho(\mathbf{s}^{\ast}_{k-1})$_ then

7 $\Omega_{k}^{a}\leftarrow\Omega_{k}^{a}\cup\\{\mathbf{s}_{k}^{(i)}\\}$;

8

9

$\mathbf{s}_{k}^{\ast}$ $\leftarrow$ CompBestState(_$\Omega_{k}^{a}$ ,
$s_{k-1}^{\ast}$_);

// Eq. (15)

$\mathbf{c}_{k}\leftarrow\mathbf{s}^{\ast}_{k}$;

// move PST center to $\mathbf{s}^{\ast}_{k}$

$\mathbf{r}_{k}$ $\leftarrow$ CompAxisLength(_$\mathbf{s}_{k}^{\ast}$ ,
$\mathbf{s}_{k-1}^{\ast}$, $\mathbf{r}_{k-1}$_);

// Eq. (16–18)

10 $\Omega_{k+1}\leftarrow$ MovePST(_$\Omega_{k}$ , $\mathbf{c}_{k}$,
$\mathbf{c}_{k-1}$_);

11 $\Omega_{k+1}\leftarrow$ RescalePST(_$\Omega_{k+1}$ , $\mathbf{c}_{k}$,
$\mathbf{r}_{k}$, $\mathbf{r}_{k-1}$_);

12 $k\leftarrow k+1$;

13

14until  _Stop condition is met_ ;

// see details in Section 4

Algorithm 1 Per-frame Pose Optimization based on PFO with PST
\begin{overpic}[width=433.62pt]{fig/img/pst_move.pdf} \end{overpic} Figure 5.
Moving and recaling PST from iteration step $k$ to $k+1$. At each step $k$, we
first identify the Advantage Particle Set (APS), i.e., $\Omega^{a}_{k}$
(shaded in red) which is a subset of the current PST $\Omega_{k}$ (blue
ellipse), and then compute the current best solution $\mathbf{s}_{k}^{\ast}$
(red dot) as the weighted average of the particles in $\Omega^{a}_{k}$. The
PST $\Omega_{k}$ is then moved to $\mathbf{s}_{k}^{\ast}$ with the new axis
length proportional to the vector
$\mathbf{v}=\mathbf{s}^{\ast}_{k}-\mathbf{s}^{\ast}_{k-1}$, thus evolving into
$\Omega_{k+1}$ (green ellipse). The same process goes for step $k+1$ shown to
the right.

#### PST move.

In each iteration step $k$, we first evaluate the fitness $\rho$ for each
particle in $\Omega_{k}$ based on Eq. (14):
$\rho(\mathbf{s})=p(g|\mathbf{s},I_{d})$. Among all the particles in
$\Omega_{k}$, we select those whose fitness is higher than
$\mathbf{s}^{\ast}_{k-1}$ (the best solution in step $k-1$) into an _Advantage
Particle Set (APS)_ :
$\Omega^{a}_{k}=\\{\mathbf{s}_{k}^{(i)}\in\Omega_{k}\mid\rho(\mathbf{s}_{k}^{(i)})>\rho(\mathbf{s}^{\ast}_{k-1})\\}$.
With APS, we define the best state at the current step $k$ as the weighted
average of all particles in APS:

(15)
$\mathbf{s}^{\ast}_{k}=\sum_{\mathbf{s}_{k}^{(i)}\in\Omega_{k}^{a}}{\bar{\omega}^{(i)}\mathbf{s}_{k}^{(i)}}$

where
$\bar{\omega}^{(i)}=\omega^{(i)}/\sum_{\mathbf{s}_{k}^{(i)}\in\Omega_{k}^{a}}{\omega^{(i)}}$
with $\omega^{(i)}=\rho(\mathbf{s}_{k}^{(i)})-\rho(\mathbf{s}^{\ast}_{k-1})$.

We then move the PST to be centered at the best state of the current step
$\mathbf{s}^{\ast}_{k}$; see Figure 5. The move in the 6D state space is
implemented by the function MovePST in Algorithm 1. Note that it is not simply
a vector space translation/addition since rotation is not closed under
addition. The details are given in the supplemental material.

#### PST rescale.

Similar to PF, a resampling step is required to avoid particle depletion. In
the context of PST, we instead rescale the spherical or ellipsoidal PST into a
new one (Figure 5). In particular, we compute the axis lengths
$\mathbf{r}_{k}$ of the current step as follows:

(16) $\displaystyle\mathbf{v}=\mathbf{s}^{\ast}_{k}-\mathbf{s}^{\ast}_{k-1},$
(17)
$\displaystyle\hat{\mathbf{r}}_{k}=(1-\rho(\mathbf{s}^{\ast}_{k}))\frac{\mathbf{v}}{\|\mathbf{v}\|}+{\bf\epsilon}.$

Here, $\mathbf{v}$ is an anisotropic attractor which drives the particles
towards the best solution $\mathbf{s}^{\ast}_{k}$ (the global best of the
particle swarm). $\hat{\mathbf{r}}_{k}$ is the (interim) vector of axis
lengths of $\Omega_{k}$, which is scaled by the inverse best fitness
$1-\rho(\mathbf{s}^{\ast}_{k})$. This means that a smaller search range is
preferable when the solution is getting better, which helps the optimization
converge more stably. ${\bf\epsilon}$ is a 6D vector of small numbers
($10^{-3}$) used to avoid degenerating PST.

The final shape of the PST is a blend between the axis lengths
$\hat{\mathbf{r}}_{k}$ and those of the previous step $\mathbf{r}_{k-1}$:

(18) $\mathbf{r}_{k}=\beta\mathbf{r}_{k-1}+(1-\beta)\hat{\mathbf{r}}_{k},$

where we use $\beta=0.1$. This sequential averaging smoothes out the variance
of axis lengths across iterations and helps to stabilize the optimization.
This is similar in spirit to the momentum mechanism in the Stochastic Gradient
Descent (SGD) methods (Ruder, 2016). Finally, we compute the scaling factor
using $\mathbf{r}_{k}$ and $\mathbf{r}_{k-1}$ and transform the particles in
PST using the function RescalePST in Algorithm 1. See the supplemental
material for details.

##  4\. Implementation details

#### TSDF normalization in likelihood estimation.

In optimizing the camera pose for a given depth frame, we have devised a
correspondence-free approach with TSDF-based maximum likelihood estimation.
Essentially, it minimizes the TSDF queries of a depth map under the camera
pose. The likelihood is estimated with Eq. (14) which involves the summation
of the TSDF values over the 3D points unprojected from _all_ depth pixels.
Generally, when performing pose estimation with frame-to-frame registration,
one should align _only_ the overlapping area observed by both the current and
the previous frames; trying to align the entire depth map as we do with Eq.
(14) may cause severe mismatching; see Figure 6(a,b) for an illustration.

In order to make the likelihood truly reflects how well the current frame
aligns with the previous one, we need to determine the overlapping area
between the two frames (note it is not frame-to-TSDF overlap which would cause
the likelihood over-evaluates misalignment). One quick option is to sum over
only those pixels whose unprojected 3D point lies in a valid (updated before)
TSDF region. This may also lead to suboptimal solutions as illustrated in
Figure 6(c,d). To identify the overlapping pixel set $O^{t}$ of depth frame
$I^{t}_{d}$ against $I^{t-1}_{d}$, we adopt the unproject-and-reproject
scheme:

(19) $O^{t}=\\{(i,j)\mid\pi^{t-1}(\pi^{t})^{-1}[(i,j)]\in I^{t-1}_{d}\textmd{
with }(i,j)\in I^{t}_{d}\\},$

where $\pi^{t}$ is the projection matrix of frame $t$ under camera pose
$\mathbf{s}^{t}$. In our implementation, the selection of valid pixel set is
done for each iteration during the optimization of $\mathbf{s}^{t}$. In
particular, when computing $O^{t}_{k}$ for iteration step $k$, we use the
projection $\pi_{k}^{t}$ built with $\mathbf{s}^{t\ast}_{k-1}$ which is the
best pose of step $k-1$. The projection $\pi^{t-1}$ simply takes
$\mathbf{s}^{t-1}_{\ast}$ which is the final optimal pose for frame $t-1$.

\begin{overpic}[width=433.62pt]{fig/img/overlap.pdf}
\put(12.0,30.5){\footnotesize(a)} \put(38.0,30.5){\footnotesize(b)}
\put(65.0,30.5){\footnotesize(c)} \put(87.5,30.5){\footnotesize(d)}
\end{overpic} Figure 6.  Finding the correct set of TSDF voxels for likelihood
estimation of $\mathbf{s}^{t}$. (a-b): When two consecutive frames $t-1$ and
$t$ have relatively small overlap (shown as highlighted voxels) due to fast
camera motion, using voxels corresponding to all unprojected 3D points of the
two depth maps would cause over-alignment of the two frames and a suboptimal
solution $\tilde{\mathbf{s}}^{t}$ (b). (c-d): If we select only those TSDF
voxels with valid values (the highlighted voxels within non-truncated area), a
suboptimal solution (d) would have a falsely high likelihood since only two
valid voxels are counted.

Based on $O^{t}_{k}$, the likelihood is estimated by summing over the
overlapping depth pixels only and normalizing the summation by the number of
those depth pixels:

(20) $p(g|\mathbf{s}^{t}_{k},I^{t}_{d})=\exp\left(-\frac{\sum_{(i,j)\in
O^{t}_{k}}{\psi(\mathbf{R}^{t}_{k}\mathbf{x}_{ij}+\mathbf{t}_{k}^{t})^{2}}}{\tau|O^{t}_{k}|}\right).$

Note that the inter-frame overlap is generally not too small so that no over-
evaluation would happen for those poses leading to small overlap: Our PST
scaling scheme ensures that the sampled transformation is usually no greater
than $10$cm in translation and $10^{\circ}$ in rotation w.r.t. the pose of the
previous step.

#### PST pre-sampling.

Since we sample particles for PST within a unit sphere in the 6D state space,
it is guaranteed that any sampled particle represents a valid rigid body
motion in $SE(3)$. The translation is measured in meter, hence each
optimization step could explore a maximum range of $1$ meter for translation
and $2\pi$ for rotation. To realize an unbiased particle sampling, we adopt
the 6D Poisson disk sampling proposed in (Bridson, 2007).

#### PST initialization.

For the first frame, PST is initialized with $\mathbf{c}_{0}$ and
$\mathbf{r}_{0}$. For each following frame $t$, we initialize PST with the
final PST center of the previous frame $t-1$, i.e.,
$\mathbf{c}_{0}^{t}=\mathbf{s}_{\ast}^{t-1}$, and with the axis lengths after
the first iteration of the previous frame, i.e.,
$\mathbf{r}_{0}^{t}=\mathbf{r}_{1}^{t-1}$; see Figure 4. The rationale of such
initialization is as follows. Since the actual solution of the current frame
is unknown a priori, a good prior of exploration region is around the best
solution of the previous frame. Setting the PST size to be
$\mathbf{r}_{1}^{t-1}$, instead of $\mathbf{r}_{0}$ or
$\mathbf{r}_{\ast}^{t-1}$, inherits the particle distribution of the previous
frame while ensuring a sufficiently large initial search range
($\mathbf{r}_{0}$ is uninformative and $\mathbf{r}_{\ast}^{t-1}$ is too
small). In Algorithm 1, the initialization is not written down but instead
summarized as a function in Line 1 since we have omitted frame index in the
description.

#### PST update.

The computation of the scaling factor in Eq. (17) depends on the evaluation of
the likelihood of the current best solution, i.e.,
$\rho(\mathbf{s}_{k}^{\ast})$. However, there are two corner cases in which
the evaluation cannot be conducted. _First,_ when TSDF has not been built in
the beginning of the reconstruction, the objective likelihood in Eq. (14)
cannot be computed. In this case, we use an initial likelihood of $0.5$. The
attractor vector in Eq. (16) is set to be isotropic
$\mathbf{v}=\mathbf{1}\in\mathbb{R}^{6}$. Hence, the PST becomes a sphere with
$\hat{\mathbf{r}}_{k}=0.5\mathbf{v}$. _Second,_ when a local optimum is
reached in the previous iteration step, the current APS is empty and hence
$\rho(\mathbf{s}_{k}^{\ast})$ cannot be computed. If we would like to search
for a better solution, we again turn the PST into a sphere by setting
$\mathbf{v}=\mathbf{1}\in\mathbb{R}^{6}$ and compute
$\hat{\mathbf{r}}_{k}=2(1-\rho(\mathbf{s}_{k-1}^{\ast}))\frac{\mathbf{v}}{\|\mathbf{v}\|}$.

\begin{overpic}[width=433.62pt]{fig/img/plot_complexity.pdf} \end{overpic}
Figure 7.  The computational time per iteration step for different PST
resolutions (number of particles) and different down-sampling rates of depth
image. The right plot is a zoom-in view of the grey box in the left plot. The
time complexity scales linearly with particle count while down-sampling can
significantly reduce the computational cost.

#### Multi-resolution PST.

The computational cost of our method is proportional to particle count in PST
(or PST resolution). While higher PST resolution makes each iteration more
time-consuming, it does increase the chance of finding a better local optimal.
In order to work with a denser PST, we opt to perform stride-based
downsampling of the depth map so that the fitness evaluation of each particle
could be accelerated. Figure 7 plots the time complexity for different PST
resolutions and depth image down-sampling rates. Low-res depth map is,
however, more noise sensitive. To balance between the resolutions of PST and
depth map, we pre-define the following three combinations of PST resolution
and depth map down-sampling rate: ($1024$, $1/8$), ($3072$, $1/16$), and
($10240$, $1/32$). The three combinations are used in turn across different
iterations. In Section 5.2, we show that such alternating resolution scheme
leads to comparable performance to high-res PST plus full-res depth map with
only $1/10$ time consumption.

#### GPU implementation.

Following most volumetric RGB-D fusion framework, our method is implemented
with the GPU. Since the fitness evaluation of the particles in PST is time-
consuming, we store both TSDF and PST in the GPU memory to facilitate fast
computation. To further accelerate the computation, we store the original PST
corresponding to the unit sphere and move and rescale this original PST in
each iteration with properly computed transformations on the fly. This way, we
avoid frequent sampling and resampling of PST particles which involves heavy
write operations. All particles are transformed and evaluated in parallel.

The fitness values of all particles are then streamed into the host CPU. They
are used to update the APS, the best solution $\mathbf{s}_{k}^{\ast}$ and the
center $\mathbf{c}_{k}$ and axis lengths $\mathbf{r}_{k}$ of PST for the
current iteration. Since the computation involves summation of a large number
of floating-point numbers, we employ the Kahan summation algorithm (Neumaier,
1974) which can significantly reduce error accumulation. The information is
then streamed back to the GPU for the next round of PST update and evaluation.
With this implementation, the amount of data streaming is minimal (up to tens
of KB), thus maximizing the utility of both GPU and CPU. Note, however, the
computation of $\mathbf{r}_{k}$ requires the fitness evaluation of
$\mathbf{s}_{k}^{\ast}$ which calls for a GPU computation. To save this extra
cost, we instead approximate its fitness by the weighted average of those in
the corresponding APS:

(21)
$\tilde{\rho}(\mathbf{s}^{\ast}_{k})=\sum_{\mathbf{s}^{(i)}_{k}\in\Omega^{a}_{k}}{\bar{\omega}^{(i)}\rho(\mathbf{s}^{(i)}_{k})}.$

The weights are the same as those in Eq. (15). Both the fitness and the
weights of APS particles have already been computed by the GPU in the previous
round. See Figure 8 for a summary of the computation and data flow on the CPU
and GPU.

\begin{overpic}[width=433.62pt]{fig/img/gpu.pdf} \end{overpic} Figure 8.  The
computation tasks of and data streaming between the CPU and the GPU. The
evaluation of particle fitness, which is the most time-consuming part, is
conducted on the GPU, while the update of PST is done on the CPU. The data
flow between is minimal (at most tens of KB).

#### Convergence criteria.

We set the maximum iteration step to $20$ to make sure a real-time frame rate
(30fps). When we set the termination condition as “APS is empty for two
consecutive iteration steps”, our optimization usually converges with less
than $4$ iterations for ordinary motion ($<1$m/s) and less than $10$
iterations for fast motion ($2\sim 4$m/s). Another stop condition is the
change of $\mathbf{s}_{k}^{\ast}$ is less than $1\times 10^{-6}$ for each of
its six dimensions.

##  5\. Results and applications

###  5.1. Benchmark

#### Public datasets.

We evaluate our method on three publicly accessible datasets including ICL-
NUIM (Handa et al., 2014), TUM RGB-D (Sturm et al., 2012) and ETH3D (Schops et
al., 2019). ICL-NUIM is a synthetic dataset comprising 4 rendered (noise
added) RGB-D sequences. For TUM RGB-D, four real captured sequences are most
often compared in the literature. Both have ordinary camera moving speed
(typically slower than 1m/s). For ETH3D, we are especially interested in the
three challenging sequences. The three sequences, prefixed with “camera
shake”, mainly contain shaking motions of RGB-D camera and exhibit increasing
moving speed from camera_shake_1 to camera_shake_3. Let us refer to these
three sequences as ETH3D-CS. All these datasets possess ground-truth camera
trajectories.

#### The FastCaMo dataset.

Given that the existing datasets contain very limited number of fast-motion
sequences, we contribute a novel and challenging dataset of RGB-D sequences
with fast camera motion, named as FastCaMo. The dataset is composed of a
synthetic part (FastCaMo-Synth) and a real captured (FastCaMo-Real) part.

FastCaMo-Synth is built upon the Replica dataset (Straub et al., 2019) which
collects $18$ highly photo-realistic 3D indoor scene reconstructions. Each
scene is represented by a dense mesh with high-resolution textures; some of
them contain planar mirror and glass reflectors. We select $10$ room-scale
scenes out of the collection. For each scene, we create a camera trajectory
through manually selecting a serial of key-frame poses and chaining them into
a smooth motion trajectory using pose interpolation similar to (Choi et al.,
2015). Along the trajectory, we generate an RGB-D sequence by sampling frames
according to camera moving speed and rendering RGB and depth images based on
the camera poses. We control the linear speed of the camera to vary in $[1,4]$
m/s and the angular speed in $[0.9,2.2]$ rad/s. For RGB image, we utilize the
renderer provided by the dataset (Straub et al., 2019). Depth maps are
rendered using the dense mesh. We also synthesize motion blur effect for each
RGB image and depth noise for each depth map using the same method as in
(Handa et al., 2014).

FastCaMo-Real contains $24$ real captured RGB-D sequences with fast camera
motion (see speed stats in Table 1) for $12$ scenes (each captured twice). The
sequences were captured using Azure Kinect DK. As for ground-truth, we opt not
to provide camera trajectories since fast moving camera is quite difficult to
track using the visual tracking systems as in TUM RGB-D and ETH3D. We
therefore offer a full and dense reconstruction scanned using the high-end
FARO Focus 3D Laser Scanner, serving as ground-truth. We can then evaluate
RGB-D reconstruction by comparing the reconstructed surface against the
ground-truth (see evaluation metrics below).

Table 1 provides the speed information of all datasets being tested. Note that
we are unable to provide the actual camera moving speed for FastCaMo-Real
since the ground-truth camera trajectories for those sequences are unknown.
Here, we give the approximated velocities estimated based on the trajectories
tracked by our method, which merely serve as a reference.

Table 1.  Statistics on camera moving speed (average linear velocity
$\bar{v}$, maximum linear velocity $v_{\text{max}}$, average angular velocity
$\bar{\omega}$ and maximum angular velocity $\omega_{\text{max}}$) for
different benchmark datasets. Note ($\ast$): The speed information for
FastCaMo-Real is approximated with our tracking results.

Item | $\bar{v}$ | $v_{\text{max}}$ | $\bar{\omega}$ | $\omega_{\text{max}}$
---|---|---|---|---
ICL-NUIM |  $0.19$m/s |  $1.84$m/s |  $0.24$rad/s |  $0.97$rad/s
TUM RGB-D |  $0.24$m/s |  $0.96$m/s |  $0.20$rad/s |  $3.37$rad/s
ETH3D-CS |  $0.38$m/s |  $0.92$m/s |  $1.94$rad/s |  $5.83$rad/s
FastCaMo-Synth |  $1.68$m/s |  $3.93$m/s |  $0.95$rad/s |  $2.18$rad/s
FastCaMo-Real $\ast$ |  $1.03$m/s |  $4.57$m/s |  $0.93$rad/s |  $5.73$rad/s

#### Evaluation metrics.

When ground-truth trajectory is available, we measure the camera tracking
quality based on the Absolute Trajectory Error (ATE) following most existing
works (Sturm et al., 2012). To estimate ATE, the trajectory to be evaluated is
first rigidly aligned to the ground-truth. ATE is then estimated as the mean
of pose differences of all frames.

Besides ATE, we also measure the per-frame pose accuracy based on Translation
Error (TE) (Sturm et al., 2012). Given the ground-truth pose
$\mathbf{T}^{t}_{E}$ and the estimated pose $\mathbf{T}^{t}_{G}$ of a frame
$t$, TE is computed as:

(22) $\text{TE}=\|trans((\mathbf{T}_{G}^{t})^{-1}\mathbf{T}^{t}_{E})\|_{2},$

where $trans(\mathbf{T})$ takes the translation part of the transformation
$\mathbf{T}$. Note that this metric does not require a pre-alignment of the
estimated and ground-truth trajectories. As long as the trajectories start
from the same initial pose of the very first frame, Translational Error can
always be estimated for the following frames in the reference system of the
first frame.

When ground-truth trajectories are unavailable, we measure the reconstruction
quality based on ground-truth surface reconstruction. Following (Dong et al.,
2019), we measure reconstruction completeness and accuracy. The reconstruction
completeness is measured as the percentage of inlier portion of the ground-
truth surface. The reconstruction accuracy is evaluated by RMS error of all
reconstructed points against the ground-truth surface. Please refer to the
supplemental material for detail definition of the two metrics.

###  5.2. Ablation Studies

We conduct a series of ablation studies to verify the necessity of several key
design choices in our method. They include the random optimization scheme (PFO
with PST), the anisotropic PST rescaling mechanism, the TSDF normalization in
likelihood/fitness estimation, and the multi-resolution PST alternation
strategy.

#### Optimization strategy (PFO with PST) – Algorithm 1.

The key to the success of our method in handing fast camera motion tracking
lies in the carefully designed random optimization scheme, i.e., PFO with PST.
To verify this core algorithmic design, we compare our full method with
_vanilla PFO (no particle swarm guidance)_ and _particle swarm optimization
(PSO; no particle swarm template)_. To make a fair comparison, all methods use
the same likelihood/fitness function and the same amount of particles.

We evaluate the three methods on $10$ sequences of FastCaMo-Synth. Each
sequence contains 6D camera motion in fast speed. In Figure 9 (left column),
we plot for the three methods the average pose likelihood/fitness (estimated
using Eq. (20)) of all frames and all sequences at different iteration steps.
The plot shows that our optimization method leads to the fastest convergence.

In the right three columns of Figure 9, we plot at each iteration step the
range of pose likelihood values of all frames for the three methods. The range
of the medium half of the likelihood values is also highlighted with a slim
box. The range of pose likelihood reveals the distribution of the particle
fitness and hence the overall uncertainty of the state estimation (solution
search). The smaller the range is, the more confidence the particle set is.
For PFO-based methods including ours, this means the entropy (diversity) of
posterior (or belief) is lower. When a good optimum is approached by the
particle set, concentrated (low entropy) is preferred than diverse (high
entropy) since the former implies a better particle utilization. Otherwise, a
diverse particle set helps to escape from local minima.

From the plots, it can be observed that our method, starting with a diverse
set of particles, gradually converges to a low entropy particle set towards a
good local optimum, thanks to the PST rescaling mechanism. Without such a
smart rescaling mechanism, the vanilla PFO usually gets stuck at a suboptimal
solution when particle utilization becomes lower. In PSO, an annealing effect
can indeed be observed. However, it tends to drive all particles toward a
common point once a good solution is found at that point, which may hurt the
exploration ability of the swarm. Our method, on the other hand, achieves a
good balance between exploration and exploitation.

\begin{overpic}[width=433.62pt]{fig/img/plot_ablation_pfo.pdf} \end{overpic}
Figure 9.  Plots of average likelihood/fitness (left column) at different
iteration steps for vanilla PFO, PSO and PFO with PST (ours). The right three
columns show the range of likelihood over all frames for the three methods.
\begin{overpic}[width=433.62pt]{fig/img/plot_2d_opt.pdf}
\put(-3.0,28.5){\rotatebox{90.0}{\footnotesize Vanilla PFO}}
\put(-3.0,19.0){\rotatebox{90.0}{\footnotesize PSO}}
\put(-3.0,-1.0){\rotatebox{90.0}{\footnotesize PFO w/ PST}} \end{overpic}
Figure 10.  2D visualization of the 6D pose optimization process for a given
frame under fast camera motion. For the three methods, we plot the evolution
of particles (dots) over iteration steps where each ellipse indicates the
particle set at a specific step and the colors encode the objective function
(Eq. (4)) values (the lower, i.e. the closer to blue, the better) of
particles. PFO with PST achieves a better trade-off between exploration and
exploitation.

Figure 10 demonstrates a 2D visualization of the process of pose optimization
by the three methods. The plots show the progressive evolution of particle
sets over iteration steps. The 6D particle sets are embedded in 2D using
sparse random projection (Li et al., 2006). Starting with the same particle
set, the three methods exhibit different exploration behaviors. PFO is able to
explore a relatively large range but the particle utilization is low. PSO
quickly gets stuck in a local optimum when initialized far from the optima.
Our method addresses both issues and results in a much better local optimum.

#### Anisotropic PST rescaling – Section 3.3 – Eq. (17).

A core design of our PFO is the anisotropic rescaling of PST guided by
particle swarm intelligence. It helps redistribute/propagate the particles
smartly so that a better optimum can be covered by the particles more
efficiently. Recall that in Eq. (17) the computation of the 6D rescaling
factor is comprised of an anisotropic part which is the anisotropic attractor
$\mathbf{v}$, and an isotropic part which is the inverse of likelihood/fitness
of the current best solution: $1-\rho(\mathbf{s}^{\ast}_{k})$. In this study,
we evaluate the effect of these two parts individually.

To reveal the effect of anisotropic rescaling in various degrees of freedom,
we create three sets of sequences with fast camera motion based on FastCaMo-
Synth. The first set contains $10$ sequences where the camera moves with only
1D transformation. In particular, the camera either translates along or
rotates about one of the three canonical axes, with a randomly varying linear
velocity in $[0.5,4]$ m/s and angular velocity in $[0.2,5]$ rad/s. The second
set includes another $10$ sequences of random 3D rotation (yaw, pitch and
roll) with the angular velocity ranging in $[0.2,5]$ rad/s. The third set of
$10$ sequences are the pre-generated ones with fast 6D camera motion.

For each of the three sets (row), we plot in Figure 11 (left column) the
average pose likelihood/fitness (see Eq. (20)) of all frames and all sequences
at different iteration steps, using methods _with anisotropic part only_ ,
_with isotropic part only_ and _with both_. The plots show that our PST
recaling with both isotropic and anisotropic parts leads to the fastest
convergence of optimization (maximization of likelihood/fitness).

\begin{overpic}[width=433.62pt]{fig/img/plot_ablation_rescaling/search_1d.pdf}
\put(-2.0,4.0){\rotatebox{90.0}{\footnotesize 1D transformation}}
\end{overpic}\begin{overpic}[width=433.62pt]{fig/img/plot_ablation_rescaling/search_3d.pdf}
\put(-3.0,8.0){\rotatebox{90.0}{\footnotesize 3D rotation}}
\end{overpic}\begin{overpic}[width=433.62pt]{fig/img/plot_ablation_rescaling/search_6d.pdf}
\put(-3.0,4.0){\rotatebox{90.0}{\footnotesize 6D transformation}}
\end{overpic}

Figure 11.  Plots of average likelihood/fitness (left column) at different
iteration steps for PST update with anisotropic rescaling only, with isotropic
rescaling only and with both. The right three columns show the range of
likelihood/fitness over all frames for the three methods, respectively. The
results are reported for the three datasets of 1D transformation (top row), 3D
rotation (middle row) and 6D transformation (bottom row).

In the right three columns of Figure 11, we plot at each iteration step the
range of likelihood values of all frames for the three methods. For the 1D
motion sequence, anisotropic rescaling leads to a more directional
distribution of particles and thus contributes more to fast convergence.
Isotropic rescaling helps concentrate particles around good optima thus
leading to a less scattered likelihoods. Our full rescaling scheme integrates
the advantages of the two. For sequences with 3D or 6D motions, however, the
difference between isotropic and anisotropic is less obvious due to the
entangling of different DoFs. Nevertheless, our full scheme still attains both
faster convergence with higher confidence.

Table 2.  Ablation study on TSDF normalization via evaluating camera tracking
accuracy (ATE) on 4 ordinary sequences of ICL-NUIM and 10 fast-motion ones of
FastCaMo (the bottom 10 rows). The best results for each sequence are
highlighted in $\mathbf{\color[rgb]{0,0,1}blue}$ color. All the three factors
are indispensable while “No Overlap” is the most critical one.

Method | No Overlap | No Normalize | No Kahan | Ours
---|---|---|---|---
ICL-NUIM kt0 |  $4.5$cm |  $3.7$cm |  $0.9$cm |  $\mathbf{\color[rgb]{0,0,1}0.8}$cm
ICL-NUIM kt1 |  $12.2$cm |  $0.7$cm |  $0.7$cm |  $\mathbf{\color[rgb]{0,0,1}0.7}$cm
ICL-NUIM kt2 |  $1.4$cm |  $1.1$cm |  $1.0$cm |  $\mathbf{\color[rgb]{0,0,1}1.0}$cm
ICL-NUIM kt3 |  $56.0$cm |  $5.4$cm |  $9.7$cm |  $\mathbf{\color[rgb]{0,0,1}4.5}$cm
Apartment_1 |  $2.8$cm |  $1.7$cm |  $1.1$cm |  $\mathbf{\color[rgb]{0,0,1}1.1}$cm
Apartment_2 |  $1.5$cm |  $1.0$cm |  $0.9$cm |  $\mathbf{\color[rgb]{0,0,1}0.9}$cm
Frl_apartment_2 |  $3.0$cm |  $2.8$cm |  $2.8$cm |  $\mathbf{\color[rgb]{0,0,1}1.9}$cm
Office_0 |  $1.1$cm |  $0.9$cm |  $0.7$cm |  $\mathbf{\color[rgb]{0,0,1}0.7}$cm
Office_1 |  $1.7$cm |  $1.2$cm |  $1.8$cm |  $\mathbf{\color[rgb]{0,0,1}1.4}$cm
Office_2 |  $5.6$cm |  $140.4$cm |  $8.3$cm |  $\mathbf{\color[rgb]{0,0,1}4.3}$cm
Office_3 |  $14.7$cm |  $13.5$cm |  $9.2$cm |  $\mathbf{\color[rgb]{0,0,1}8.0}$cm
Hotel_0 |  $2.0$cm |  $160.4$cm |  $1.8$cm |  $\mathbf{\color[rgb]{0,0,1}1.5}$cm
Room_0 |  $2.8$cm |  $3.5$cm |  $3.3$cm |  $\mathbf{\color[rgb]{0,0,1}2.3}$cm
Room_1 |  $7.8$cm |  $4.4$cm |  $4.5$cm |  $\mathbf{\color[rgb]{0,0,1}4.0}$cm

#### TSDF normalization – Section 4 – Eq. (20).

We evaluate the necessity of TSDF normalization in likelihood estimation,
which is composed of three design choices: 1) the determination of overlapping
pixel set $O^{t}$ (Eq. (19)), 2) the normalization of the summation of TSDF by
set $O^{t}$, and 3) the adoption of the Kahan algorithm (Neumaier, 1974) in
computing the summation of TSDF. We test our method without each one of the
three components:

  * •

No Overlap: The normalization of TSDF summation is performed over the full
frame of depth map, i.e., replacing $O^{t}$ by $I^{t}_{d}$ in Eq. (20).

  * •

No Normalize: The likelihood is estimated without normalizing the summation of
TSDF, i.e., replacing Eq. (20) by Eq. (14).

  * •

No Kahan: The summation of TSDF is conducted directly, i.e., without using the
Kahan algorithm.

The evaluation is conducted on both ordinary sequences ICL-NUIM and our fast-
motion ones FastCaMo-Synth. Table 2 compares the tracking accuracy of our
method and the various baselines. It can be seen that “No Overlap” causes the
most accuracy drop among the three baselines, suggesting its importance in
likelihood estimation. For Office_2 and Hotel_0, “No Normalize” produces large
tracking error which can be attributed to exactly the cases of Figure 6(d),
i.e., a very small portion of depth map lying within the valid range of TSDF
due to very large rotation. Although relatively less significant, “No Kahan”
does affect the final tracking accuracy, hinting that numerical precision is a
non-ignorable factor.

#### Multi-res PST alternation – Section 4.

To balance between the particle density of PST and the computational cost of
fitness evaluation, we opt to work with three different combinations of PST
resolution and depth map down-sampling rate: ($1024$, $1/8$), ($3072$,
$1/16$), and ($10240$, $1/32$). The three combinations are used alternatively
across iterations. In Figure 12, we evaluate such multi-res alternation scheme
by comparing the accuracy and efficiency of camera tracking against various
single resolution options on FastCaMo-Synth. From the figure, the success rate
of our alternation scheme is close to or higher than those high-res settings
for almost all ATE thresholds, while our computational cost is at the same
level as the low-res options. This verifies the effectiveness of our
alternation scheme. It is the nature of swarm-guided PFO that enables this
improvement: It is unnecessary to use a high-res PST at every step since 1)
the best solution found by high-res PSTs would be passed along and inherited
by the following iteration steps, and 2) the high-res depth maps associated
with low-res PSTs yields more noise-robust likelihood estimation.

\begin{overpic}[width=209.0025pt]{fig/img/plot_multires/plot_multires_1.pdf}
\end{overpic}\begin{overpic}[width=215.07646pt]{fig/img/plot_multires/plot_multires_2.pdf}
\end{overpic}

Figure 12.  Comparing tracking accuracy and efficiency of our method
(alternation between three resolution combinations) against various single
resolution settings. Our method achieves the highest accuracy (success rate
for different ATE thresholds) with a comparable cost to low-res the settings.

\begin{overpic}[width=140.92517pt]{fig/img/comp_plot_eth3d/camera_shake_1.pdf}
\put(27.0,76.0){\footnotesize{camera\\_shake\\_1}}
\end{overpic}\begin{overpic}[width=140.92517pt]{fig/img/comp_plot_eth3d/camera_shake_2.pdf}
\put(27.0,76.0){\footnotesize{camera\\_shake\\_2}}
\end{overpic}\begin{overpic}[width=140.92517pt]{fig/img/comp_plot_eth3d/camera_shake_3.pdf}
\put(27.0,76.0){\footnotesize{camera\\_shake\\_3}} \end{overpic}

Figure 13.  Comparing percentage of frames under increasing tolerance of
translation error of per-frame pose on the three camera shaking sequences of
ETH3D. Our method achieves much higher success rate of per-frame pose tracking
than BundleFusion and ElasticFusion.

###  5.3. Quantitative Comparisons

We quantitatively evaluate our method against several state-of-the-art methods
for both ordinary and fast-motion sequences.

#### Comparison on ordinary benchmarks.

In most existing datasets, the camera motion is usually slower than 1m/s. For
those sequences, our method is able to achieve a comparable accuracy of camera
tracking. Table 3 compares ATE on the four sequences of ICL-NUIM between our
method and the state-of-the-art online (DVO-SLAM (Kerl et al., 2013), RGB-D
SLAM (Endres et al., 2012), MRSMap (Stückler and Behnke, 2014), Kintinuous
(Whelan et al., 2012), VoxelHashing (Nießner et al., 2013), ElasticFusion
(Whelan et al., 2015), BundleFusion (Dai et al., 2017)) and offline (Redwood
(Choi et al., 2015)) methods. Our method achieves comparable accuracy to the
best-performing method BundleFusion which involves a global pose optimization
by bundle adjustment. The kt3 sequence is the most challenging one where there
is barely no color or geometric features in some frames. Our method attains a
decent accuracy even without any global optimization. Similar comparative
results can also be observed for TUM RGB-D; _see the supplemental material_.

Table 3.  Comparing the accuracy (ATE) of camera tracking on the four RGB-D
sequences of ICL-NUIM. The best and the second best results for each sequence
are highlighted in $\mathbf{\color[rgb]{0,0,1}blue}$ and
$\mathbf{\color[rgb]{0,0.4,0}green}$ colors, respectively.

Sequence | kt0 | kt1 | kt2 | kt3
---|---|---|---|---
DVO SLAM |  $10.4$cm |  $2.9$cm |  $19.1$cm |  $15.2$cm
RGB-D SLAM |  $2.6$cm |  $0.8$cm |  $1.8$cm |  $43.3$cm
MRSMap |  $20.4$cm |  $22.8$cm |  $18.9$cm |  $109$cm
Kintinuous |  $7.2$cm |  $\mathbf{\color[rgb]{0,0.4,0}0.5}$cm |  $\mathbf{\color[rgb]{0,0.4,0}1.0}$cm |  $35.5$cm
VoxelHashing |  $1.4$cm |  $\mathbf{\color[rgb]{0,0,1}0.4}$cm |  $1.8$cm |  $12.0$cm
ElasticFusion |  $0.9$cm |  $0.9$cm |  $1.4$cm |  $10.6$cm
Redwood (rigid) |  $25.6$cm |  $3.0$cm |  $3.3$cm |  $6.1$cm
BundleFusion |  $\mathbf{\color[rgb]{0,0,1}0.6}$cm |  $\mathbf{\color[rgb]{0,0,1}0.4}$cm |  $\mathbf{\color[rgb]{0,0,1}0.6}$cm |  $\mathbf{\color[rgb]{0,0,1}1.1}$cm
RoseFusion |  $\mathbf{\color[rgb]{0,0.4,0}0.8}$cm |  $0.7$cm |  $\mathbf{\color[rgb]{0,0.4,0}1.0}$cm |  $\mathbf{\color[rgb]{0,0.4,0}4.5}$cm

Table 4.  Comparing the accuracy (ATE) of camera tracking on the three
challenging RGB-D sequences of ETH3D. The best and the second best results for
each sequence are highlighted in $\mathbf{\color[rgb]{0,0,1}blue}$ and
$\mathbf{\color[rgb]{0,0.4,0}green}$ colors, respectively. ‘–’ indicates that
the tracking was failed.

Sequence | camera_shake_1 | camera_shake_2 | camera_shake_3
---|---|---|---
BAD SLAM | – | – | –
DVO-SLAM |  $9.40$cm | – | –
ORB-SLAM2 | – |  $6.89$cm | –
ElasticFusion |  $8.44$cm | – | –
BundleFusion |  $\mathbf{\color[rgb]{0,0.4,0}5.17}$cm |  $\mathbf{\color[rgb]{0,0.4,0}3.49}$cm | –
RoseFusion |  $\mathbf{\color[rgb]{0,0,1}0.62}$cm |  $\mathbf{\color[rgb]{0,0,1}1.35}$cm |  $\mathbf{\color[rgb]{0,0,1}4.67}$cm

#### Comparison on fast-motion benchmark – ETH3D.

The advantage of our method is best reflected on fast-motion sequences. We
first conduct a comparison on the three camera shake sequences of ETH3D-CS. As
reported in Table 1, the average angular velocity of camera motion of these
sequences is nearly $10$ times faster than ICL-NUIM and TUM RGB-D. Our method
is compared to BAD-SLAM (Schops et al., 2019), DVO-SLAM, ORB-SLAM2 (Mur-Artal
and Tardós, 2017), ElasticFusion and BundleFusion. ETH3D-CS contains IMU data
which was not used by any of the methods being compared. The results are
reported in Table 4. Our method was able to reconstruct all the three
sequences with an acceptable tracking accuracy while none of the other methods
could succeed on all. camera_shake_3 is the fastest sequence on which all
other method failed while ours achieves $4.6$cm ATE.

In the plots of Figure 13, we provide a breakdown study of per-frame pose
accuracy for each camera shake sequence. In particular, we measure the frame-
wise pose error using Translation Error (TE) and plot the percentage of frames
whose pose TE is below different thresholds. BundleFusion drops frames which
are lost tracking. We count the dropped frames as failed for all TE thresholds
since the mechanism of frame dropping in BundleFusion is more involved than
TE. Therefore, the comparison with BundleFusion may not be absolutely fair; we
provide the results serving only as a reference. It can be seen that our
method achieves consistently more accurate per-frame pose estimation on all
the three sequences.

\begin{overpic}[width=84.98914pt]{fig/img/comp_plot_fcm/tracking_apartment_1.pdf}
\put(37.0,73.0){\footnotesize{apartment\\_1}}
\end{overpic}\begin{overpic}[width=84.98914pt]{fig/img/comp_plot_fcm/tracking_apartment_2.pdf}
\put(37.0,73.0){\footnotesize{apartment\\_2}}
\end{overpic}\begin{overpic}[width=84.98914pt]{fig/img/comp_plot_fcm/tracking_frl_apartment_2.pdf}
\put(37.0,73.0){\footnotesize{apartment\\_3}}
\end{overpic}\begin{overpic}[width=84.98914pt]{fig/img/comp_plot_fcm/tracking_hotel_0.pdf}
\put(44.0,73.0){\footnotesize{hotel\\_0}}
\end{overpic}\begin{overpic}[width=84.98914pt]{fig/img/comp_plot_fcm/tracking_office_0.pdf}
\put(43.0,73.0){\footnotesize{office\\_0}}
\end{overpic}\begin{overpic}[width=84.98914pt]{fig/img/comp_plot_fcm/tracking_office_1.pdf}
\put(43.0,73.0){\footnotesize{office\\_1}}
\end{overpic}\begin{overpic}[width=84.98914pt]{fig/img/comp_plot_fcm/tracking_office_2.pdf}
\put(43.0,73.0){\footnotesize{office\\_2}}
\end{overpic}\begin{overpic}[width=84.98914pt]{fig/img/comp_plot_fcm/tracking_office_3.pdf}
\put(43.0,73.0){\footnotesize{office\\_3}}
\end{overpic}\begin{overpic}[width=84.98914pt]{fig/img/comp_plot_fcm/tracking_room_0.pdf}
\put(45.0,73.0){\footnotesize{room\\_0}}
\end{overpic}\begin{overpic}[width=84.98914pt]{fig/img/comp_plot_fcm/tracking_room_1.pdf}
\put(45.0,73.0){\footnotesize{room\\_1}} \end{overpic}

Figure 14.  Comparing percentage of frames under increasing tolerance of
translation error of per-frame pose on ten sequences of our FastCaMo-Synth
dataset. Our method achieves significantly higher success rate of per-frame
pose tracking than BundleFusion and ElasticFusion.

#### Comparison on fast-motion benchmark – FastCaMo.

Table 5 provides a comparison on FastCaMo-Synth. Based on the ground-truth
trajectories of this synthetic dataset, we compare our method with
ElasticFusion and BundleFusion on the ten sequences. Note that, in what
follows, we mainly compare to these two methods in the various fast-motion
tests since they are the representative state-of-the-arts in point-based and
volumetric fusion, respectively.

Compare to the two baselines, our method demonstrates significantly higher
tracking accuracy for all the sequences except Room_1. The Room_1 sequence is
quite challenging since the empty room contains barely no color or geometric
feature. BundleFusion is able to reconstruct a small portion of the room with
good tracking accuracy while dropping $88.8\%$ of the frames. On the contrary,
our method and ElasticFusion do not drop frame. Similar situation is also
found in Room_0, Office_2 and Office_3, on which our method achieves
successful reconstruction. Figure 16 shows three examples of visual
reconstruction results for FastCaMo-Synth.

Table 5.  Comparing the accuracy (ATE) of camera tracking on the ten scenes of
FastCaMo-Synth. The best results for each sequence are highlighted in
$\mathbf{\color[rgb]{0,0,1}blue}$ color. ‘–’ indicates that the tracking was
failed. The frame dropping rate of BundleFusion is high for these sequences
(e.g., $88.8\%$ of the frames of Room_1 were dropped by BundleFusion) while
ElasticFusion and our method did not drop any frame. Note, however, ATE is
reported for reconstructed frames only for all methods being compared.

Method | ElasticFusion | BundleFusion | Ours
---|---|---|---
Apartment_1 |  $40.9$cm |  $4.6$cm |  $\mathbf{\color[rgb]{0,0,1}1.1}$cm
Apartment_2 |  $40.7$cm |  $2.2$cm |  $\mathbf{\color[rgb]{0,0,1}1.0}$cm
Frl_apartment_2 |  $43.8$cm |  $83.6$cm |  $\mathbf{\color[rgb]{0,0,1}1.9}$cm
Office_0 |  $22.3$cm |  $2.7$cm |  $\mathbf{\color[rgb]{0,0,1}0.7}$cm
Office_1 |  $2.3$cm |  $17.3$cm |  $\mathbf{\color[rgb]{0,0,1}1.4}$cm
Office_2 |  $65.9$cm |  $93.0$cm |  $\mathbf{\color[rgb]{0,0,1}4.3}$cm
Office_3 |  $94.3$cm |  $253.5$cm |  $\mathbf{\color[rgb]{0,0,1}8.0}$cm
Hotel_0 |  $43.8$cm |  $65.2$cm |  $\mathbf{\color[rgb]{0,0,1}1.5}$cm
Room_0 | – | – |  $\mathbf{\color[rgb]{0,0,1}2.3}$cm
Room_1 |  $31.0$cm |  $\mathbf{\color[rgb]{0,0,1}0.6}$cm |  $4.0$cm

Table 6. Comparing reconstruction completeness (Compl.) and accuracy (Acc.) of
ElasticFusion, BundleFusion and our method over the FastCaMo-Real dataset. The
best results for each sequence are highlighted in
$\mathbf{\color[rgb]{0,0,1}blue}$ color. We used the default parameters of the
two alternatives; better results could be obtained by tuning their parameters.

| ElasticFusion | BundleFusion | Ours
---|---|---|---
| Compl. | Acc. | Compl. | Acc. | Compl. | Acc.
Apartment_I | $22.1\%$ |  $7.7$cm | $34.2\%$ |  $6.4$cm | $\mathbf{\color[rgb]{0,0,1}84.3\%}$ |  $\mathbf{\color[rgb]{0,0,1}4.8}$cm
Apartment_II | $15.0\%$ |  $7.2$cm | $25.2\%$ |  $5.2$cm | $\mathbf{\color[rgb]{0,0,1}86.6\%}$ |  $\mathbf{\color[rgb]{0,0,1}4.2}$cm
Lab | $15.1\%$ |  $7.3$cm | $16.9\%$ |  $5.4$cm | $\mathbf{\color[rgb]{0,0,1}91.6\%}$ |  $\mathbf{\color[rgb]{0,0,1}4.8}$cm
Stairwell | $11.7\%$ |  $8.3$cm | $14.4\%$ |  $5.8$cm | $\mathbf{\color[rgb]{0,0,1}82.8\%}$ |  $\mathbf{\color[rgb]{0,0,1}5.4}$cm
Gym | $61.1\%$ |  $7.7$cm | $12.4\%$ |  $\mathbf{\color[rgb]{0,0,1}5.1}$cm | $\mathbf{\color[rgb]{0,0,1}61.8\%}$ |  $6.9$cm
Lounge_I | $10.7\%$ |  $7.8$cm | $7.7\%$ |  $6.1$cm | $\mathbf{\color[rgb]{0,0,1}93.5\%}$ |  $\mathbf{\color[rgb]{0,0,1}4.5}$cm
Lounge_II | $8.8\%$ |  $9.0$cm | $1.8\%$ |  $9.4$cm | $\mathbf{\color[rgb]{0,0,1}87.9\%}$ |  $\mathbf{\color[rgb]{0,0,1}5.4}$cm
Studio | $36.1\%$ |  $7.2$cm | $63.8\%$ |  $\mathbf{\color[rgb]{0,0,1}4.9}$cm | $\mathbf{\color[rgb]{0,0,1}65.1\%}$ |  $\mathbf{\color[rgb]{0,0,1}4.9}$cm
Meeting_room | $17.9\%$ |  $8.5$cm | $20.1\%$ |  $6.9$cm | $\mathbf{\color[rgb]{0,0,1}90.0\%}$ |  $\mathbf{\color[rgb]{0,0,1}5.8}$cm
Office | $16.4\%$ |  $8.3$cm | $6.1\%$ |  $5.6$cm | $\mathbf{\color[rgb]{0,0,1}67.7\%}$ |  $\mathbf{\color[rgb]{0,0,1}5.4}$cm
Workshop_I | $11.6\%$ |  $9.2$cm | $25.4\%$ |  $\mathbf{\color[rgb]{0,0,1}5.6}$cm | $\mathbf{\color[rgb]{0,0,1}55.9\%}$ |  $5.7$cm
Workshop_II | $16.3\%$ |  $7.0$cm | $51.6\%$ |  $\mathbf{\color[rgb]{0,0,1}5.4}$cm | $\mathbf{\color[rgb]{0,0,1}66.4\%}$ |  $\mathbf{\color[rgb]{0,0,1}5.4}$cm

Similar to Figure 13, Figure 14 provides breakdown analyses of frame-wise pose
accuracy for the ten sequences. These plots also reflect the difficulty of the
above-mentioned sequences; see the slowly growing percentage. Our method again
performs consistently better than the two baseline methods. For Room_1, our
curve is notably higher than BundleFusion indicating that our method attains
an overall better tracking when all frames are counted in.

In Table 6, we also conduct a comparison on $12$ real captured sequences of
FastCaMo-Real. See the results on the other $12$ sequences in the supplemental
material. Since these sequences only possess ground-truth reconstruction by
LiDAR, we evaluate the reconstruction quality, i.e., the completeness and
accuracy w.r.t. the ground-truth surfaces. As the reconstruction accuracy
measures the RMS error over only the overlapping (inlier) regions between the
reconstructed and the ground-truth surfaces, the numbers of the three methods
are close to each other although ours does not involve any post-processing of
global pose optimization or loop closure as the two alternative. In Table 6,
we set the threshold of inlier to $15$cm. When the threshold is set to $5$cm,
the average error is $1\sim 3$cm, with about $10\%$ drop in completeness. The
reconstruction quality is best exposed by completeness on which our method is
consistently and significantly better than the two alternatives. The visual
results of reconstruction for this dataset can be found in Figure 17.

\begin{overpic}[width=390.25534pt]{fig/img/comp_plot_rss.pdf}
\put(18.0,41.0){\footnotesize{FastCaMo-Synth} }
\put(72.0,41.0){\footnotesize{ICL-NUIM} } \end{overpic} Figure 15.  Plots of
average objective function values at different iteration steps for our method
(blue) and (Bylow et al., 2013) (orange). For our method, we also show the
range of objective of all particles over all frames. The results are reported
for both FastCaMo-Synth (left) and ICL-NUIM (right).

#### Comparison with (Bylow et al., 2013).

Our method is closely related to (Bylow et al., 2013) in which a similar
objective was defined based on the summation of TSDF over the unprojected 3D
points of a depth map (Eq. (4)). This makes their method correspondence-free
too. Different from our method, however, they pursue a gradient descent
approach and employ the Gauss-Newton method to minimize the summation of TSDF.
This nonlinear least square approach finds difficulty in handling large
rotation as shown in many related works (Huang et al., 2006). Moreover, the
gradient can be undefined when the unprojected 3D points lie out of the valid
range of TSDF when the camera undergoes a large transformation.

In Figure 15 (left column), we plot the objective function values (Eq. (4);
the lower the better) of all frames and all sequences at different iteration
steps. For our method, we plot both the average values and the range of values
of all particles. For their method, we simply plot the actual objective
function values at each iteration step. Their values are mostly larger than
the medium half of our particles (see the slim boxes). This demonstrates that
our method minimizes the objective with a significantly faster convergence for
both of the two datasets.

###  5.4. Qualitative Results

#### Visual comparison of reconstruction.

We provide visual results on both synthetic and real fast-motion datasets.
Figure 16 shows the reconstructions on three sequences of FastCaMo-Synth (see
the full set of results in the supplemental material). For each sequence, we
show the reconstruction results along with the tracked camera trajectories for
ROSEFusion (left), ElasticFusion (middle) and BundleFusion (right). The
tracked trajectories are overlaid on top of the ground-truth ones for visual
contrasting. It can be seen that our trajectory matches well to the ground-
truth. For BundleFusion, the trajectory segments corresponding to those
remaining frames (did not get dropped) also conform well against the ground-
truth.

\begin{overpic}[width=433.62pt]{fig/img/gallery_synth.pdf}
\put(9.0,74.0){\footnotesize ROSEFusion} \put(43.0,74.0){\footnotesize
ElasticFusion} \put(75.0,74.0){\footnotesize BundleFusion}
\put(0.5,70.5){\footnotesize{Room\\_1}}
\put(0.5,45.5){\footnotesize{Office\\_1}}
\put(0.5,20.2){\footnotesize{Frl\\_apartment\\_2}} \end{overpic} Figure 16.
3D reconstruction results and tracked camera trajectories (red curves) for
three fast-motion sequences of FastCaMo-Synth. For each sequence, we compare
the results of ROSEFusion (left), ElasticFusion (middle) and BundleFusion
(right). The ground-truth trajectories (green curves) are overlaid for
reference purpose.

Figure 17 is a gallery of visual results for FastCaMo-Real, in a similar
layout to Figure 16. The linear velocity of camera movement (estimated based
on our tracking) is color-coded (refer to Figure 1 for color bar) along the
trajectories. Note how our method is able to successfully reconstruct most of
the sequences on which the other two methods failed. The Stairwell sequence is
especially challenging since it contains repetitive structure but no prominent
color features. Nevertheless, our method succeeds on it with good
reconstruction quality. See also Table 6 for quantitative measurement. The Gym
sequence is also quite challenging due to the large mirror on the wall. The
mirror reflectance causes drastically missing/erroneous depth such that our
method produces some drift along the wall which is any loop closure or global
optimization mechanism.

\begin{overpic}[width=433.62pt]{fig/img/gallery.pdf}
\put(6.0,74.5){\footnotesize ROSEFusion} \put(21.0,74.5){\footnotesize
ElasticFusion} \put(37.0,74.5){\footnotesize BundleFusion}
\put(56.0,74.5){\footnotesize ROSEFusion} \put(72.0,74.5){\footnotesize
ElasticFusion} \put(88.0,74.5){\footnotesize BundleFusion}
\put(0.0,73.0){\footnotesize{Apartment\\_I}}
\put(51.0,73.0){\footnotesize{Apartment\\_II}}
\put(0.0,60.6){\footnotesize{Lab}} \put(51.0,60.6){\footnotesize{Stairwell}}
\put(0.0,48.5){\footnotesize{Gym}} \put(51.0,48.5){\footnotesize{Lounge\\_I}}
\put(0.0,36.0){\footnotesize{Lounge\\_II}}
\put(51.0,36.0){\footnotesize{Studio}}
\put(0.0,23.7){\footnotesize{Meeting\\_room}}
\put(51.0,23.7){\footnotesize{Office}}
\put(0.0,11.3){\footnotesize{Workshop\\_I}}
\put(51.0,11.3){\footnotesize{Workshop\\_II}} \end{overpic} Figure 17.
Gallery of 3D reconstruction results along with tracked camera trajectories
for the twelve real captured fast-motion sequences of FastCaMo-Real. For each
sequence, we compare the results of ROSEFusion (left), ElasticFusion (middle)
and BundleFusion (right). The linear velocity of camera movement is color-
coded (refer to Figure 1 for color bar) along the trajectories.

#### Visualization of optimization process.

In Figure 18, we present a visualization of the per-frame pose optimization
process. Given the current frame (the green point cloud) with an initial pose,
the goal is to optimize its pose to align it against the previous frame (the
red point cloud). For each example, the initial and final configurations are
shown at the two ends and the progressive evolution of PST is visualized in-
between. In the upper part of the evolution sequence, we visualize the
landscape of objective (Eq. (4)) via 2D Isomap embedding (Tenenbaum et al.,
2000) of the 6D solution space, as well as the exploration path of our PST.
The lower part shows the evolution of the 6D PST visualized as a 3D ellipsoid.
The axis lengths of the 3D ellipsoid depict the rescaling factors along the
three dimensions corresponding to translation and the color map encodes the
scales along three rotational dimensions. The color map on the ellipsoid can
be seen as the Gauss map of camera orientations of all particles. Note that
the rotation here refers to relative rotation w.r.t. the pose of the previous
step. Using relative rotation makes the color maps well aligned and the color
change easy to observe.

The visualization clearly demonstrates the power of our random optimization
scheme. Although the optimization landscape is highly non-convex due to large
inter-frame transformations, our PFO guided by PST can always finds a good
local optimum efficiently. It enables good quality frame-to-frame alignment
without needing photometric or geometric feature detection and correspondence.

\begin{overpic}[width=433.62pt]{fig/img/plot_3d_opt.pdf} \end{overpic} Figure
18.  Visualization of pose optimization process on three representative frames
selected from kt3 of ICL-NUIM, camera_shake_3 of ETH3D-CS and Lab of FastCaMo-
Real, respectively. In each row, we show two frames with initial poses, the
evolution of PST during optimization, and the final alignment result. The
upper part of the evolution sequence shows the optimization landscape (colored
plot) and the exploration path (green curve) of PST. The lower part shows the
evolution of the 6D PST visualized with a 3D ellipsoid.

###  5.5. Complexity and Runtime Analysis

The time complexity of our random optimization is $O(NM)$ ($N$ particles and
$M$ depth pixels) for each iteration. In our method, all particle fitness
evaluation could be performed in parallel on the GPU (Section 4). We have
implemented our core algorithm in C++ and CUDA. Both the main optimization
pipeline and the volumetric fusion run on a workstation with an Intel® CoreTM
i7-5930K CPU @ 3.50GHz $\times$ 12 with 32G RAM and an Nvidia GeForce RTX 2080
SUPER GPU with 8G memory. To enable flexible scanning, we implemented a front-
end program running on a laptop for RGB-D capturing, compressing, and
streaming to the workstation via WiFi. Our pipeline runs with a framerate of
$30$Hz for all shown test sequences. In fact, we control the time budget of
random optimization process within $30$ms to maintain a $30$Hz framerate. This
time slot allows the optimization to run for at least $20$ iterations which is
well-sufficient for all the sequences tested as our method converges with $4$
iterations for ordinary sequences and $10$ for fast-motion ones. The time for
volumetric depth fusion is $3$ms per frame. The readers are welcomed to watch
our accompanying video.

###  5.6. Limitation and Failure Cases

Our method has several limitations. _Firstly_ , since our method is purely
geometrically-based, it would naturally fail when no prominent geometric
feature can be found in a serial of consecutive frames. Figure 19(a) shows
such a cases where the camera is scanning towards a flat wall (fr3_nst of TUM
RGB-D). When the camera motion is not too fast to be contaminated by motion
blur, one can always complement our method with photometric based pose
optimization. _Secondly_ , when depth is severely missing due to reflectance
or absorbance of light, our method would fail to track and fuse; see Figure
19(b). _Thirdly_ , while it is hard to gauge the upper limit of camera moving
speed, our method could still fail under extremely fast motion (e.g. $\sim
5$m/s in the example of Figure 19(c)). _Finally_ , a more fundamental issue is
that our current method does not include a loop closure detection and global
pose optimization. Although we have tested scanning an indoor space of about
$300$m2 for $10$K frames without introducing observable drift, it is almost
impossible to track without drifting when the sequence becomes extremely long
especially some challenging frames were encountered along the way. The Gym
sequence in Figure 17 is a failure example of such kind. Nonetheless, our
method can of course be enhanced by loop closure and/or global pose
optimization.

\begin{overpic}[width=433.62pt]{fig/img/failure_case.pdf}
\put(14.0,-3.0){\footnotesize(a)} \put(48.0,-3.0){\footnotesize(b)}
\put(83.0,-3.0){\footnotesize(c)} \end{overpic} Figure 19.  Three typical
failure cases of ROSEFusion: (a) almost no geometric feature (flat wall), (b)
severe depth missing (transparent glass) and (c) too fast camera motion ($\sim
5$m/s).

##  6\. Discussion and conclusions

With our work, we wish to bring it to the community’s attention that online
dense reconstruction under fast camera motion is a practically useful problem
and might demand a new paradigm of solution. The traditional approaches are
mostly based on feature matching plus gradient descent. They do find
difficulties in handling fast-motion camera tracking due to motion blur of
color images and highly nonlinear/non-convex optimization landscape.

Our attempt achieves a satisfactory outcome thanks to two major design
choices. _First_ , we pursue a depth-only approach and define an effective
objective function based on depth-to-TSDF conformance, thus avoiding explicit
feature detection and correspondence. _Second_ , we propose to solve the per-
frame optimization based on a novel particle-filter-based random optimization
with swarm guidance, leading to a robust and efficient algorithm.

Our work serves merely as a starting point, which we hope would inspire a rich
set future directions:

  * •

How to integrate loop closure detection and global pose optimization into the
random optimization framework? It is interesting to study random optimization
based pose graph optimization or bundle adjustment when large transformations
are involved.

  * •

How to unify traditional approach and random optimization into a single
framework? When camera motion slows down, it is of course a good option to
enhance camera tracking with photometric features. Here, how to bridge and
switch smoothly between the two technique is worth of investigating.

  * •

How to fuse multi-modal input to further improve the robustness of fast-motion
camera tracking?

  * •

How to realize autonomous reconstruction with mobile robots or drones with our
technique? There are several technical issues to address, e.g., how to
integrate our method with online motion planning (Xu et al., 2015, 2017; Liu
et al., 2018; Dong et al., 2019).

  * •

It is interesting to extend our random optimization to distributed bundle
adjustment (Eriksson et al., 2016; Natesan Ramamurthy et al., 2017) for
collaborative reconstruction by a network of robots or drones. Here, the small
overlap between distributed nodes also calls for a robust pose optimization.

  * •

Another promising and interesting direction is to explore the combination of
random optimization and deep priors for online and/or semantic scene
reconstruction (Zhang et al., 2020; Huang et al., 2020; Nie et al., 2020).

###### Acknowledgements.

We thank the anonymous reviewers for their valuable comments and suggestions.
We are grateful to Yuefeng Xi and Yao Chen for their effort in the preparation
of the FastCaMo dataset. This work was supported in part by National Key
Research and Development Program of China (2018AAA0102200), NSFC (61532003,
62002376) and NUDT Research Grants (ZK19-30).

## References

  * (1)
  * Andrieu and Doucet (2002) Christophe Andrieu and Arnaud Doucet. 2002.  Particle filtering for partially observed Gaussian state space models.  _Journal of the Royal Statistical Society: Series B (Statistical Methodology)_ 64, 4 (2002), 827–836.
  * Arnaud and Mémin (2005) Elise Arnaud and Etienne Mémin. 2005.  An efficient Rao-Blackwellized particle filter for object tracking. In _IEEE International Conference on Image Processing 2005_ , Vol. 2. IEEE, II–426.
  * Bridson (2007) Robert Bridson. 2007\.  Fast Poisson disk sampling in arbitrary dimensions.  _SIGGRAPH sketches_ 10 (2007), 1.
  * Bylow et al. (2013) Erik Bylow, Jürgen Sturm, Christian Kerl, Fredrik Kahl, and Daniel Cremers. 2013.  Real-time camera tracking and 3D reconstruction using signed distance functions. In _Robotics: Science and Systems_ , Vol. 2. 2.
  * Chen et al. (2013) Jiawen Chen, Dennis Bautembach, and Shahram Izadi. 2013\.  Scalable real-time volumetric surface reconstruction.  _ACM Transactions on Graphics (ToG)_ 32, 4 (2013), 1–16.
  * Choi and Christensen (2012) Changhyun Choi and Henrik I Christensen. 2012.  Robust 3D visual tracking using particle filtering on the special Euclidean group: A combined approach of keypoint and edge features.  _The International Journal of Robotics Research_ 31, 4 (2012), 498–519.
  * Choi et al. (2015) Sungjoon Choi, Qian-Yi Zhou, and Vladlen Koltun. 2015\.  Robust reconstruction of indoor scenes. In _Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition_. 5556–5565.
  * Cui et al. (2019) Zhaopeng Cui, Lionel Heng, Ye Chuan Yeo, Andreas Geiger, Marc Pollefeys, and Torsten Sattler. 2019\.  Real-time dense mapping for self-driving vehicles using fisheye cameras. In _2019 International Conference on Robotics and Automation (ICRA)_. IEEE, 6087–6093.
  * Curless and Levoy (1996) Brian Curless and Marc Levoy. 1996.  A volumetric method for building complex models from range images. In _Proc. of SIGGRAPH_. 303–312.
  * Dai et al. (2017) Angela Dai, Matthias Nießner, Michael Zollhöfer, Shahram Izadi, and Christian Theobalt. 2017\.  BundleFusion: Real-time Globally Consistent 3D Reconstruction using On-the-fly Surface Reintegration.  _ACM Transactions on Graphics (TOG)_ 36, 3 (2017), 24\.
  * Deng et al. (2019) Xinke Deng, Arsalan Mousavian, Yu Xiang, Fei Xia, Timothy Bretl, and Dieter Fox. 2019\.  Poserbpf: A rao-blackwellized particle filter for 6d object pose tracking.  _arXiv preprint arXiv:1905.09304_ (2019).
  * Dong et al. (2019) Siyan Dong, Kai Xu, Qiang Zhou, Andrea Tagliasacchi, Shiqing Xin, Matthias Nießner, and Baoquan Chen. 2019.  Multi-robot collaborative dense scene reconstruction.  _ACM Transactions on Graphics (TOG)_ 38, 4 (2019), 1–16.
  * Endres et al. (2012) Felix Endres, Jürgen Hess, Nikolas Engelhard, Jürgen Sturm, Daniel Cremers, and Wolfram Burgard. 2012.  An evaluation of the RGB-D SLAM system. In _2012 IEEE International Conference on Robotics and Automation_. IEEE, 1691–1696.
  * Engel et al. (2014) Jakob Engel, Thomas Schöps, and Daniel Cremers. 2014\.  LSD-SLAM: Large-scale direct monocular SLAM. In _European conference on computer vision_. Springer, 834–849.
  * Engel et al. (2015) Jakob Engel, Jörg Stückler, and Daniel Cremers. 2015\.  Large-scale direct SLAM with stereo cameras. In _2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 1935–1942.
  * Engel et al. (2013) Jakob Engel, Jurgen Sturm, and Daniel Cremers. 2013\.  Semi-dense visual odometry for a monocular camera. In _Proceedings of the IEEE international conference on computer vision_. 1449–1456.
  * Eriksson et al. (2016) Anders Eriksson, John Bastian, Tat-Jun Chin, and Mats Isaksson. 2016.  A consensus-based framework for distributed bundle adjustment. In _Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition_. 1754–1762.
  * Forster et al. (2016) Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza. 2016.  On-Manifold Preintegration for Real-Time Visual–Inertial Odometry.  _IEEE Transactions on Robotics_ 33, 1 (2016), 1–21.
  * Forster et al. (2014) Christian Forster, Matia Pizzoli, and Davide Scaramuzza. 2014\.  SVO: Fast semi-direct monocular visual odometry. In _2014 IEEE international conference on robotics and automation (ICRA)_. IEEE, 15–22.
  * Gallego et al. (2019) Guillermo Gallego, Tobi Delbruck, Garrick Orchard, Chiara Bartolozzi, Brian Taba, Andrea Censi, Stefan Leutenegger, Andrew Davison, Jörg Conradt, Kostas Daniilidis, et al. 2019\.  Event-based vision: A survey.  _arXiv preprint arXiv:1904.08405_ (2019).
  * Gil et al. (2010) Arturo Gil, Óscar Reinoso, Mónica Ballesta, and Miguel Juliá. 2010.  Multi-robot visual SLAM using a Rao-Blackwellized particle filter.  _Robotics and Autonomous Systems_ 58, 1 (2010), 68–80.
  * Gordon et al. (1993) Neil J Gordon, David J Salmond, and Adrian FM Smith. 1993\.  Novel approach to nonlinear/non-Gaussian Bayesian state estimation. In _IEE proceedings F (radar and signal processing)_ , Vol. 140. IET, 107–113.
  * Grisetti et al. (2007) Giorgio Grisetti, Cyrill Stachniss, and Wolfram Burgard. 2007\.  Improved techniques for grid mapping with rao-blackwellized particle filters.  _IEEE transactions on Robotics_ 23, 1 (2007), 34–46.
  * Grisettiyz et al. (2005) Giorgio Grisettiyz, Cyrill Stachniss, and Wolfram Burgard. 2005\.  Improving grid-based slam with rao-blackwellized particle filters by adaptive proposals and selective resampling. In _Proceedings of the 2005 IEEE international conference on robotics and automation_. IEEE, 2432–2437.
  * Handa et al. (2014) Ankur Handa, Thomas Whelan, John McDonald, and Andrew J Davison. 2014.  A benchmark for RGB-D visual odometry, 3D reconstruction and SLAM. In _2014 IEEE international conference on Robotics and automation (ICRA)_. IEEE, 1524–1531.
  * Hansard et al. (2012) Miles Hansard, Seungkyu Lee, Ouk Choi, and Radu Patrice Horaud. 2012.  _Time-of-flight cameras: principles, methods and applications_.  Springer Science & Business Media.
  * Henry et al. (2014) Peter Henry, Michael Krainin, Evan Herbst, Xiaofeng Ren, and Dieter Fox. 2014.  RGB-D mapping: Using depth cameras for dense 3D modeling of indoor environments. In _Experimental robotics_. Springer, 477–491.
  * Huang et al. (2020) Jiahui Huang, Shi-Sheng Huang, Haoxuan Song, and Shi-Min Hu. 2020\.  DI-Fusion: Online Implicit 3D Reconstruction with Deep Priors.  _arXiv preprint arXiv:2012.05551_ (2020).
  * Huang et al. (2006) Jin Huang, Xiaohan Shi, Xinguo Liu, Kun Zhou, Li-Yi Wei, Shang-Hua Teng, Hujun Bao, Baining Guo, and Heung-Yeung Shum. 2006.  Subspace gradient domain mesh deformation.  In _ACM SIGGRAPH 2006 Papers_. 1126–1134.
  * Izadi et al. (2011) Shahram Izadi, David Kim, Otmar Hilliges, David Molyneaux, Richard Newcombe, Pushmeet Kohli, Jamie Shotton, Steve Hodges, Dustin Freeman, Andrew Davison, and Andrew Fitzgibbon. 2011.  KinectFusion: Real-time 3D Reconstruction and Interaction Using a Moving Depth Camera. In _UIST_. 559–568.
  * Ji et al. (2008) Chunlin Ji, Yangyang Zhang, Mengmeng Tong, and Shengxiang Yang. 2008.  Particle filter with swarm move for optimization. In _International Conference on Parallel Problem Solving from Nature_. Springer, 909–918.
  * Kahler et al. (2015) O. Kahler, V. A. Prisacariu, C. Y. Ren, X. Sun, P. H. S Torr, and D. W. Murray. 2015\.  Very High Frame Rate Volumetric Integration of Depth Images on Mobile Device.  _IEEE Trans. Vis. & Computer Graphics (ISMAR)_ 22, 11 (2015).
  * Keller et al. (2013) Maik Keller, Damien Lefloch, Martin Lambers, Shahram Izadi, Tim Weyrich, and Andreas Kolb. 2013\.  Real-time 3d reconstruction in dynamic scenes using point-based fusion. In _2013 International Conference on 3D Vision-3DV 2013_. IEEE, 1–8.
  * Kerl et al. (2013) Christian Kerl, Jürgen Sturm, and Daniel Cremers. 2013\.  Dense visual SLAM for RGB-D cameras. In _2013 IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2100–2106.
  * Klein and Murray (2007) Georg Klein and David Murray. 2007.  Parallel tracking and mapping for small AR workspaces. In _2007 6th IEEE and ACM international symposium on mixed and augmented reality_. IEEE, 225–234.
  * Kuemmerle et al. (2011) Rainer Kuemmerle, Giorgio Grisetti, Hauke Strasdat, Kurt Konolige, and Wolfram Burgard. 2011.  g2o: A General Framework for Graph Optimization. In _Proc. ICRA_.
  * Laidlow et al. (2017) Tristan Laidlow, Michael Bloesch, Wenbin Li, and Stefan Leutenegger. 2017.  Dense rgb-d-inertial slam with map deformations. In _2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 6741–6748.
  * Lee (2014) Seungkyu Lee. 2014\.  Time-of-flight depth camera motion blur detection and deblurring.  _IEEE Signal Processing Letters_ 21, 6 (2014), 663–666.
  * Leutenegger et al. (2015) Stefan Leutenegger, Simon Lynen, Michael Bosse, Roland Siegwart, and Paul Furgale. 2015.  Keyframe-based visual–inertial odometry using nonlinear optimization.  _The International Journal of Robotics Research_ 34, 3 (2015), 314–334.
  * Li et al. (2006) Ping Li, Trevor J Hastie, and Kenneth W Church. 2006.  Very sparse random projections. In _Proceedings of the 12th ACM SIGKDD international conference on Knowledge discovery and data mining_. 287–296.
  * Liu et al. (2016) Bin Liu, Shi Cheng, and Yuhui Shi. 2016.  Particle filter optimization: A brief introduction. In _International Conference on Swarm Intelligence_. Springer, 95–104.
  * Liu et al. (2018) Ligang Liu, Xi Xia, Han Sun, Qi Shen, Juzhan Xu, Bin Chen, Hui Huang, and Kai Xu. 2018.  Object-Aware Guidance for Autonomous Scene Reconstruction.  _ACM Trans. on Graph. (SIGGRAPH)_ 37, 4 (2018).
  * Mourikis and Roumeliotis (2007) Anastasios I Mourikis and Stergios I Roumeliotis. 2007.  A multi-state constraint Kalman filter for vision-aided inertial navigation. In _Proceedings 2007 IEEE International Conference on Robotics and Automation_. IEEE, 3565–3572.
  * Mur-Artal et al. (2015) Raul Mur-Artal, Jose Maria Martinez Montiel, and Juan D Tardos. 2015\.  ORB-SLAM: a versatile and accurate monocular SLAM system.  _IEEE transactions on robotics_ 31, 5 (2015), 1147–1163.
  * Mur-Artal and Tardós (2017) Raul Mur-Artal and Juan D Tardós. 2017.  Orb-slam2: An open-source slam system for monocular, stereo, and rgb-d cameras.  _IEEE Transactions on Robotics_ 33, 5 (2017), 1255–1262.
  * Natesan Ramamurthy et al. (2017) Karthikeyan Natesan Ramamurthy, Chung-Ching Lin, Aleksandr Aravkin, Sharath Pankanti, and Raphael Viguier. 2017\.  Distributed bundle adjustment. In _Proceedings of the IEEE International Conference on Computer Vision Workshops_. 2146–2154.
  * Neumaier (1974) A. Neumaier. 1974\.  Rounding Error Analysis of Some Methods for Summing Finite Sums.  _Zeitschrift für Angewandte Mathematik und Mechanik_ 54, 1 (1974), 39–51.
  * Newcombe et al. (2011a) Richard A Newcombe, Andrew J Davison, Shahram Izadi, Pushmeet Kohli, Otmar Hilliges, Jamie Shotton, David Molyneaux, Steve Hodges, David Kim, and Andrew Fitzgibbon. 2011a.  KinectFusion: Real-time dense surface mapping and tracking. In _Proc. IEEE Int. Symp. on Mixed and Augmented Reality_. 127–136.
  * Newcombe et al. (2011b) Richard A Newcombe, Steven J Lovegrove, and Andrew J Davison. 2011b.  DTAM: Dense tracking and mapping in real-time. In _2011 international conference on computer vision_. IEEE, 2320–2327.
  * Nie et al. (2020) Yinyu Nie, Ji Hou, Xiaoguang Han, and Matthias Nießner. 2020.  RfD-Net: Point Scene Understanding by Semantic Instance Reconstruction.  _arXiv preprint arXiv:2011.14744_ (2020).
  * Nießner et al. (2014) Matthias Nießner, Angela Dai, and Matthew Fisher. 2014\.  Combining Inertial Navigation and ICP for Real-time 3D Surface Reconstruction.. In _Eurographics (Short Papers)_. Citeseer, 13–16.
  * Nießner et al. (2013) M. Nießner, M. Zollhöfer, S. Izadi, and M. Stamminger. 2013.  Real-time 3D Reconstruction at Scale using Voxel Hashing.  _ACM Trans. on Graph. (SIGGRAPH Asia)_ 32, 6 (2013), 169\.
  * Nieto et al. (2016) Marcos Nieto, Andoni Cortés, Oihana Otaegui, Jon Arróspide, and Luis Salgado. 2016\.  Real-time lane tracking using Rao-Blackwellized particle filter.  _Journal of Real-Time Image Processing_ 11, 1 (2016), 179–191.
  * Pollefeys et al. (2008) Marc Pollefeys, David Nistér, J-M Frahm, Amir Akbarzadeh, Philippos Mordohai, Brian Clipp, Chris Engels, David Gallup, S-J Kim, Paul Merrell, et al. 2008\.  Detailed real-time urban 3d reconstruction from video.  _International Journal of Computer Vision_ 78, 2 (2008), 143–167.
  * Pradeep et al. (2013) Vivek Pradeep, Christoph Rhemann, Shahram Izadi, Christopher Zach, Michael Bleyer, and Steven Bathiche. 2013.  MonoFusion: Real-time 3D reconstruction of small scenes with a single web camera. In _2013 IEEE International Symposium on Mixed and Augmented Reality (ISMAR)_. IEEE, 83–88.
  * Ruder (2016) Sebastian Ruder. 2016\.  An overview of gradient descent optimization algorithms.  _arXiv preprint arXiv:1609.04747_ (2016).
  * Saurer et al. (2016) Olivier Saurer, Marc Pollefeys, and Gim Hee Lee. 2016\.  Sparse to dense 3d reconstruction from rolling shutter images. In _Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition_. 3337–3345.
  * Scaramuzza and Zhang (2019) Davide Scaramuzza and Zichao Zhang. 2019.  Visual-inertial odometry of aerial robots.  _arXiv preprint arXiv:1906.03289_ (2019).
  * Schmidt and Niemann (2001) Jochen Schmidt and Heinrich Niemann. 2001.  Using Quaternions for Parametrizing 3-D Rotations in Unconstrained Nonlinear Optimization.. In _Vmv_ , Vol. 1. Citeseer, 399–406.
  * Schops et al. (2019) Thomas Schops, Torsten Sattler, and Marc Pollefeys. 2019\.  BAD SLAM: Bundle adjusted direct rgb-d slam. In _Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition_. 134–144.
  * Shi and Eberhart (1999) Yuhui Shi and Russell C Eberhart. 1999.  Empirical study of particle swarm optimization. In _Proceedings of the 1999 congress on evolutionary computation-CEC99 (Cat. No. 99TH8406)_ , Vol. 3. IEEE, 1945–1950.
  * Straub et al. (2019) Julian Straub, Thomas Whelan, Lingni Ma, Yufan Chen, Erik Wijmans, Simon Green, Jakob J Engel, Raul Mur-Artal, Carl Ren, Shobhit Verma, et al. 2019\.  The Replica dataset: A digital replica of indoor spaces.  _arXiv preprint arXiv:1906.05797_ (2019).
  * Stückler and Behnke (2014) Jörg Stückler and Sven Behnke. 2014.  Multi-resolution surfel maps for efficient dense 3D modeling and tracking.  _Journal of Visual Communication and Image Representation_ 25, 1 (2014), 137–147.
  * Sturm et al. (2012) Jürgen Sturm, Nikolas Engelhard, Felix Endres, Wolfram Burgard, and Daniel Cremers. 2012\.  A benchmark for the evaluation of RGB-D SLAM systems. In _2012 IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 573–580.
  * Tenenbaum et al. (2000) Joshua B Tenenbaum, Vin De Silva, and John C Langford. 2000\.  A global geometric framework for nonlinear dimensionality reduction.  _science_ 290, 5500 (2000), 2319–2323.
  * Thrun (2002) Sebastian Thrun. 2002\.  Probabilistic robotics.  _Commun. ACM_ 45, 3 (2002), 52–57.
  * Triggs et al. (1999) Bill Triggs, Philip F McLauchlan, Richard I Hartley, and Andrew W Fitzgibbon. 1999.  Bundle adjustment: a modern synthesis. In _International workshop on vision algorithms_. Springer, 298–372.
  * Whelan et al. (2012) Thomas Whelan, Michael Kaess, Maurice Fallon, Hordur Johannsson, John Leonard, and John McDonald. 2012.  Kintinuous: Spatially Extended KinectFusion. In _RSS Workshop on RGB-D: Advanced Reasoning with Depth Cameras_.
  * Whelan et al. (2015) Thomas Whelan, Stefan Leutenegger, Renato F Salas-Moreno, Ben Glocker, and Andrew J Davison. 2015\.  ElasticFusion: Dense SLAM without a pose graph. In _Proc. Robotics: Science and Systems_.
  * Xu et al. (2015) Kai Xu, Hui Huang, Yifei Shi, Hao Li, Pinxin Long, Jiannong Caichen, Wei Sun, and Baoquan Chen. 2015\.  Autoscanning for Coupled Scene Reconstruction and Proactive Object Analysis.  _ACM Trans. on Graph._ 34, 6 (2015), 177.
  * Xu et al. (2017) Kai Xu, Lintao Zheng, Zihao Yan, Guohang Yan, Eugene Zhang, Matthias Nießner, Oliver Deussen, Daniel Cohen-Or, and Hui Huang. 2017.  Autonomous Reconstruction of Unknown Indoor Scenes Guided by Time-varying Tensor Fields.  _ACM Transactions on Graphics 2017 (TOG)_ (2017).
  * Zhang et al. (2017) Chi Zhang, Amirhossein Taghvaei, and Prashant G Mehta. 2017\.  A controlled particle filter for global optimization.  _arXiv preprint arXiv:1701.02413_ (2017).
  * Zhang et al. (2020) Jiazhao Zhang, Chenyang Zhu, Lintao Zheng, and Kai Xu. 2020\.  Fusion-aware point convolution for online semantic 3d scene segmentation. In _Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition_. 4534–4543.