# Online and Certifiably Correct
Visual Odometry and Mapping

Devansh R. Agrawal,1∗ Rajiv Govindjee,1 Jiangbo Yu,2 Anurekha Ravikumar,3 and
Dimitra Panagou1,3 The authors would like to acknowledge the support of the
National Science Foundation through the Award No. 1942907. This work was
partially supported by Amazon.1Department of Aerospace Engineering, University
of Michigan, 1320 Beal Ave, Ann Arbor, MI 48109, USA.2Department of Mechanical
Engineering, University of Michigan, 2350 Hayward St, Ann Arbor, MI 48109,
USA.3Department of Robotics, University of Michigan, 2505 Hayward St, Ann
Arbor, MI 48109, USA.∗Correspondence<EMAIL_ADDRESS>
###### Abstract

This paper proposes two new algorithms for certified perception in safety-
critical robotic applications. The first is a Certified Visual Odometry
algorithm, which uses a RGBD camera with bounded sensor noise to construct a
visual odometry estimate with provable error bounds. The second is a Certified
Mapping algorithm which, using the same RGBD images, constructs a Signed
Distance Field of the obstacle environment, always safely underestimating the
distance to the nearest obstacle. This is required to avoid errors due to VO
drift. The algorithms are demonstrated in hardware experiments, where we
demonstrate both running online at 30FPS. The methods are also compared to
state-of-the-art techniques for odometry and mapping.

##  I Introduction

Accurate and reliable perception, state estimation, and mapping are critical
components of a robotic system. When operating safety-critical robotic
systems, the planners and controllers rely on the outputs of the perception
module to determine whether a planned trajectory or control action is safe.
Over the last decade, many methods have been developed to certify that a
controller satisfies safety specifications prescribed a priori, e.g. [1, 2].
If the specification is to avoid obstacles, and these obstacles can only be
sensed online, it can be more natural to address safety constraints using the
planning module [3, 4, 5]. However these approaches tend to assume perfect
information from the perception system, an unrealistic assumption that can
lead to safety violations.

A perception module returns a pose estimate and a map representing the
obstacles in an environment. The representations can take many forms,
including an ESDF [6, 7], occupancy log-odds [8] or NERFs [9]. These methods
construct “best-estimate” maps, and do not quantify the error. Without error
bounds, the planners/controllers are unable to guarantee safety.

In state estimation, there are some methods to determine error bounds. A
Kalman Filter for example quantifies the state estimation uncertainty in terms
of a covariance ellipsoid. There is also growing interest in certified
perception techniques, i.e., algorithms that provably recover the globally
optimal solution or in some cases those that can provide error bounds with
respect to the optimal solution. For example, the algorithm in [10] provably
returns the global optimum to a pose-graph optimization problem, by
reformulating it into a convex optimization problem. The algorithm in [11] can
determine the location of a robot in a convex 2D environment with error
bounds. On the other hand, the accuracy of (uncertified) perception algorithms
have been improving in recent years, and many experimental demonstrations show
good performance in GPS-denied environments [12, 13, 14, 15]. To the best of
our knowledge, no formal error analysis is available for these methods.

Currently, robust safety-critical planners/controllers are designed to handle
specific forms of uncertainty. These include additive dynamics disturbances
that are either bounded or stochastic [16, 17, 18], or state estimation errors
that are, again, bounded or stochastic [19, 20]. Therefore our perception
module must return estimates and error bounds compatible with planners and
controllers.

This paper takes two steps towards the goal of certified perception-planning-
control algorithms. First, we propose a new Certified Visual Odometry (C-VO)
algorithm for which (under appropriate assumptions) we can establish a bounded
odometry error, i.e. error bounds on the relative pose between successive
camera frames. As these errors accumulate over time, the position estimate
will deviate from the ground truth position of the robot, as is commonly
observed in Visual Odometry (VO) systems [12, 13]. If using mapping frameworks
like VoxBlox [6], this can lead to some occupied regions marked free in the
map. Therefore, our second key contribution is a Certified  (C-ESDF)
algorithm. Building on the framework of [7], we introduce a deflation step,
ensuring that the C-ESDF always underestimates the distance to the nearest
obstacle in a body-fixed frame. To the best of the authors’ knowledge, this is
the first algorithm that can certify the VO and ESDF outputs of a perception
stack. Finally, we have performed experiments to demonstrate that the proposed
algorithms can run in real-time.

The remainder of the paper is structured as follows. In Section II we
formalize the problem, and in Section III we describe conceptually the key
components of our method. The main theoretical contributions are detailed in
Sections IV and V. These theoretical contributions are assembled into a
complete algorithm in Section VI. Finally in Section VII we report experiments
demonstrating the proposed methods.

#### Notation

$\mathbb{N}=\\{0,1,2,...\\}$ is the set of natural numbers.
$\mathbb{R},\mathbb{R}_{\geq 0},\mathbb{R}_{>0}$ denote reals, non-negative
reals, and positive reals. $\mathbb{S}^{3}$ is the set of unit quaternions.
$\mathbb{SO}(3)$ is the special orthogonal group.
$\mathbb{SE}(3)=\mathbb{SO}(3)\times\mathbb{R}^{3}$ is the special Euclidean
group. $\left\|v\right\|_{p}$ denotes the $p$-norm of a vector, and
$\left\|v\right\|$ denotes the 2-norm. For matrices $A\in\mathbb{R}^{n\times
m}$, $\left\|A\right\|$ denotes the induced 2-norm, and $\left\|A\right\|_{F}$
denotes the Frobenius norm. All eigenvectors are assumed to be unit-norm. A
unit quaternion $q\in\mathbb{S}^{3}$ is denoted
$q=\begin{bmatrix}q_{1}&q_{2}&q_{3}&q_{4}\end{bmatrix}^{T}$, where the scalar
component is last. The inverse is
$q^{-1}=\begin{bmatrix}-q_{1}&-q_{2}&-q_{3}&q_{4}\end{bmatrix}^{T}$. The
quaternion product is $q_{c}=q_{a}\circ
q_{b}=\Omega_{1}(q_{a})q_{b}=\Omega_{2}(q_{b})q_{a}$, where

$\displaystyle\Omega_{1}(q)=\begin{bmatrix}q_{4}&-q_{3}&q_{2}&q_{1}\\\
q_{3}&q_{4}&-q_{1}&q_{2}\\\ -q_{2}&q_{1}&q_{4}&q_{3}\\\
-q_{1}&-q_{2}&-q_{3}&q_{4}\end{bmatrix},$
$\displaystyle\Omega_{2}(q)=\begin{bmatrix}q_{4}&q_{3}&-q_{2}&q_{1}\\\
-q_{3}&q_{4}&q_{1}&q_{2}\\\ q_{2}&-q_{1}&q_{4}&q_{3}\\\
-q_{1}&-q_{2}&-q_{3}&q_{4}\end{bmatrix}.$

Every $q\in\mathbb{S}^{3}$ is associated with a rotation $R\in\mathbb{SO}(3)$,
$\overline{Ra}=q\circ\overline{a}\circ q^{-1}$, where
$\overline{v}=\begin{bmatrix}v^{T}&0\end{bmatrix}^{T}\in\mathbb{R}^{4}$ for
any $v\in\mathbb{R}^{3}$. Therefore, we have the useful properties
$\Omega_{1}(q^{-1})=\Omega_{1}^{T}(q)$,
$\Omega_{2}(q^{-1})=\Omega_{2}^{T}(q)$, and $q\circ\overline{a}\circ
q^{-1}=\Omega_{2}^{T}(q)\Omega_{1}(q)\bar{a}$.

A reference frame is a set of three orthonormal basis vectors and an origin.
When a point $p$ is expressed in reference frame $M$, it is denoted
$p|^{M}\in\mathbb{R}^{3}$. Two frames $A,B$ are be related by a
rototranslation $(R,t)\in\mathbb{SE}(3)$, $p|^{B}=R_{A}^{B}p|^{A}+t_{A}^{B}$.
This paper uses the inertial frame $I$, and a mapping frame $M$. The body
frame $B_{k}$ denotes the body-fixed reference frame at timestep $k$, i.e.,
when the $k$-th RGBD image is produced. See also Table III in the appendix.

##  II Problem Statement

The goal in this paper is to identify a subset of $\mathbb{R}^{3}$ in the
body-frame that is certifiably obstacle-free. We constrain ourselves to using
an RGB-Depth (RGBD) camera without access to an Inertial Measurement Unit
(IMU).111Using an IMU can improve the accuracy of localization algorithms and
provide robustness against changes in lighting conditions [21, 22]. However,
certifying error bounds is challenging due to the IMU biases. Incorporating an
IMU is considered future work. The depth sensor generates a measured
pointcloud of the obstacles within its Field of View (FoV). The position of
each point in the pointcloud could have some measurement error. We assume this
error is bounded with a known bound.222According to manufacturer
specifications, each point of the pointcloud has an absolute position error of
2% of the distance from the camera [23]. Empirical studies have suggested a
quadratic relationship [24]. Then, we have the following problem statement:

######  Problem 1.

Consider a robotic system with an onboard RGBD camera operating at a fixed
frame rate in a static environment. Suppose the depth camera produces
pointclouds with a bounded position error in each point. Identify a subset
$\mathcal{F}\subset\mathbb{R}^{3}$ that is guaranteed to be obstacle free.

We assume that if a point $p\in\mathbb{R}^{3}$ is occupied, and within the
camera’s FoV, it will be detected as an obstacle. This is a common implicit
assumption in the mapping literature. Note, an infrared depth camera often
fails to detect transparent obstacles (e.g., windows and glass doors). Such
issues are beyond the scope of this paper.

##  III Method Overview

Our solution decomposes the certified perception problem into two steps, the
certified state estimation problem and the certified mapping problem, as
depicted in Figure 1. Although separate, these modules have been designed to
integrate together. Here we describe the modules conceptually, and explain our
choices in section III-C.

Figure 1: Block diagram describing our certified perception modules. Given
successive RGBD frames, we first use the visual odometry (orange) module to
compute a relative pose estimate, and the associated error bounds. These are
used in the mapping (purple) module to construct a 3D map of the obstacle
geometry. Using the error bounds computed in C-VO, we can compute the ESDF
deflation that is sufficient to ensure correctness.

###  III-A C-VO

First, we propose a new method for C-VO. By _certified_ we mean that our VO
algorithm produces an estimate of the odometry between successive timesteps,
_and_ produces an upper-bound on the maximum error between the estimated
odometry and the true odometry.

We adapt the robust pointcloud registration technique in [25] to estimate the
rototranslation between successive timesteps. Using the bounded error of the
sensed pointcloud, we derive an upperbound on the rototranslation estimation
error. More precisely in Lemma 3 we determine a Frobenius norm bound on the
rotation error, and in Lemma 4 we determine a two-norm bound on the
translation error.

###  III-B C-ESDF

Second, we propose an algorithm to construct a C-ESDF to represent the world.
New depth images are integrated into into a voxelized representation of the
world, where each voxel stores the distance to the nearest obstacle, an ESDF.
As the robot moves through the environment, the accumulated VO drift implies
that the mapping reference frame shifts relative to the inertial frame. Since
the obstacles are not static in the mapping frame, the ESDF can become
incorrect.

To account for this, we introduce a deflation step to [7]. We derive a
recursive guarantee that if the C-ESDF safely underestimates the distance to
every obstacle at timestep $k$, after deflating the C-ESDF it will again
safely underestimate the Signed Distance Field (SDF) at timestep $k+1$.

We accomplish this by, at every frame, decrementing the C-ESDF at each voxel
based on the C-VO error bound. The deflation amount is different at each
voxel, and is computed from the error bounds produced by the C-VO algorithm as
described in Section V. This effectively causes the system to ‘forget’ parts
of the map not been observed recently. The result is a map that is always
guaranteed to be safe for use in path planning and control.

###  III-C Why Pointcloud Registration and ESDFs?

The proposed approach has some key properties that enables certification
guarantees. Here we compare our strategy to possible alternatives.

_Compared to Epipolar VO [26]:_ A common technique in VO is to use Epipolar
constraints to determine the rototranslation between frames. However due to
the geometric properties used in deriving the algorithm, it is not clear how
to compute error bounds for this algorithm.

_Compared to Octomap[8]:_ An Octomap stores the occupancy log-odds. However
since odometry error is bounded by a norm ball, it is unclear how to update
the log-odds.

_Compared to Safe Flight Corridors (SFC) [27]:_ Again, based on a norm-ball
error bound, the algebra necessary to update the SFC polyhedron is not
clear.333Consider a SFC $F=\\{x\in\mathbb{R}^{3}:Ax\leq b\\}$. If the
rototranslation from frame $k$ to $k+1$ is $(R,t)\in\mathbb{SE}(3)$, the new
safe region is $F^{\prime}=\\{x\in\mathbb{R}^{3}:AR^{T}x\leq b+AR^{T}t\\}$.
However if there is uncertainty in $(R,t)$, to the best of our knowledge,
there is no analytic method to compute $F^{\prime}$.

_Compared to inflating the obstacles:_ One could maintain a list of observed
obstacles, and inflate their radius based on the accumulated VO error. With
this approach however, the spheres could grow to occupy all of
$\mathbb{R}^{3}$, preventing any new region from being certified safe.
Instead, we choose to deflate safe regions. Although this can lead to a
situation where no subset of $\mathbb{R}^{3}$ is certifiably safe, as new
depth images are received, new regions can be added to the certified-free set.

Thus, we choose to use the proposed C-VO and C-ESDF algorithms. Naturally,
from the C-ESDF a SFC or an Octomap could be extracted as required.

##  IV Theory: Certified Visual Odometry (C-VO)

Here we describe our proposed rototranslation estimation algorithm. Recall we
assume a bounded sensor measurement error of the position of each point in the
pointcloud.

Consider successive timesteps $k,k+1$. First, we use classical feature
detection and optical flow algorithms to identify features in RGB image $k$,
and identify their corresponding position in image $k+1$ [28, 29]. Once
projected to 3D, we have two pointclouds with a list of correspondences. This
process is not perfect, and can lead to outliers. Thus, we design a robust
rototranslation algorithm that is robust to a small number of outliers. See
Section VI for additional implementation details and the effect of outliers.
The VO problem is now a pointcloud registration problem: the rototranslation
between the pointclouds is equivalent to the rototranslation between
successive body frames. Thus the robust rototranslation estimation problem is
as follows:

######  Problem 2.

Let $\mathcal{A}=\\{a_{i}\\}_{i=1}^{N},\mathcal{B}=\\{b_{i}\\}_{i=1}^{N}$ be
two sets of points $(a_{i},b_{i}\in\mathbb{R}^{3})$ such that
$\mathcal{A},\mathcal{B}$ are related by the model

$\displaystyle
b_{i}=Ra_{i}+t+\epsilon_{i},\quad\left\|\epsilon_{i}\right\|\leq\delta_{i}$
(1)

where $(R,t)\in\mathbb{SE}(3)$ is the true rototranslation between pointclouds
$\mathcal{A},\mathcal{B}$, and $\epsilon_{i}\in\mathbb{R}^{3}$ is sensor
noise, with known bounds $\delta_{i}>0$ for each $i\in\\{1,...,N\\}$.
Determine $(\hat{R},\hat{t})\in\mathbb{SE}(3)$ that solves the following
problem:

$\displaystyle\underset{\begin{subarray}{c}\hat{R}\in\mathbb{SO}(3),\hat{t}\in\mathbb{R}^{3}\end{subarray}}{\text{argmin}}\sum_{i=1}^{N}\min\left(\left\|b_{i}-\hat{R}a_{i}-\hat{t}\right\|^{2},\delta_{i}^{2}\right)$
(2)

Equation (2) is a Truncated Least Squares (TLS) problem, since any term with
$\left\|b_{i}-\hat{R}a_{i}-\hat{t}\right\|>\delta_{i}$ will only contribute a
fixed amount $\delta_{i}^{2}$ to the cost. This truncated cost allows the
optimization to be robust to outliers. Although nonconvex, through a sequence
of reformulations, we obtain a computationally-efficient method to solve this
problem. The method is largely inspired by [25], but there are some key
details (e.g., the computation of the error bounds) that are different. For
completeness, we explain the full procedure.

###  IV-A Rotation Estimation

First we decouple the rotation and translation. Notice for any pair of points
$i,j\in\\{1,...,N\\}$,

$\displaystyle b_{i}-b_{j}=R(a_{i}-a_{j})+\epsilon_{i}-\epsilon_{j}$ (3)

is independent of $t$. Define $a_{ij}=a_{i}-a_{j}$, $b_{ij}=b_{i}-b_{j}$,
$\epsilon_{ij}=\epsilon_{i}-\epsilon_{j}$. Then,

$\displaystyle\left\|\epsilon_{ij}\right\|\leq\left\|\epsilon_{i}\right\|+\left\|\epsilon_{j}\right\|\leq\delta_{ij}$
(4)

where $\delta_{ij}=\delta_{i}+\delta_{j}$. To capture the pairs, we construct
an undirected graph $G=(V,E)$, where $V=\\{1,...,N\\}$ and $E\subset V\times
V$. Ideally, $G$ should be a complete graph, with $\left|E\right|=N(N-1)/2$
edges. In practice, other graph topologies can be used to improve
computational performance, as discussed in section VI-A.

######  Problem 3.

Consider the setup in 2. Let $G=(V,E)$ be a graph connecting pairs of points.
Determine $\hat{R}\in\mathbb{SO}(3)$ that solves

$\displaystyle\underset{\begin{subarray}{c}\hat{R}\in\mathbb{SO}(3)\end{subarray}}{\text{argmin}}\sum_{(i,j)\in
E}\min\left(\left\|b_{ij}-\hat{R}a_{ij}\right\|^{2},\delta_{ij}^{2}\right)$
(5)

To solve Problem 3, we use Graduated-Nonconvexity (GNC) [30], the details of
which are omitted in the interest of space. For GNC to be efficient, we
require a fast method to solve an associated Weighted Least Squares (WLS)
problem. For Problem 3, the WLS problem has an analytic solution:

######  Lemma 1.

Consider WLS problem

$\displaystyle\underset{\begin{subarray}{c}\hat{R}\in\mathbb{SO}(3)\end{subarray}}{\text{argmin}}\sum_{(i,j)\in
E}w_{ij}\left\|b_{ij}-\hat{R}a_{ij}\right\|^{2}$ (6)

given weights $w_{ij}\in[0,1]$. Define444Recall
$\overline{a}_{ij}=\begin{bmatrix}a_{ij}^{T}&0\end{bmatrix}^{T}$,
$\overline{b}_{ij}=\begin{bmatrix}b_{ij}^{T}&0\end{bmatrix}^{T}$. See Section
I for $\Omega_{1},\Omega_{2}$.

$\displaystyle Q_{ij}$
$\displaystyle=\Omega_{1}^{T}(\overline{b}_{ij})\Omega_{2}(\overline{a}_{ij})+\Omega_{2}^{T}(\overline{a}_{ij})\Omega_{1}(\overline{b}_{ij})$
(7a) $\displaystyle Q$ $\displaystyle=-\sum_{(i,j)\in E}w_{ij}Q_{ij}$ (7b)

where $Q_{ij},Q\in\mathbb{R}^{4\times 4}$ are symmetric matrices. Let

$\displaystyle\hat{q}=\texttt{eigvec}(Q)\in\mathbb{R}^{4}$ (8)

be the (unit-norm) eigenvector corresponding to the smallest eigenvalue of
$Q$. Then, $\hat{q}$ is the unit quaternion corresponding to the rotation
matrix $\hat{R}$, the solution to (6).

###### Proof.

Adapted from [25]. See appendix B-A. ∎

Notice the WLS problem (6) requires determining an eigenvector of a symmetric
real matrix of fixed size $\mathbb{R}^{4\times 4}$ independent of $N$. This
makes 3 efficient.

Next, we establish the rotation error bound.

######  Lemma 2.

Consider the setup in 3. Let $\hat{R}\in\mathbb{SO}(3)$ solve 3. Then the
rotation error is bounded by

$\displaystyle\left\|R-\hat{R}\right\|_{F}\leq\epsilon_{R}=\sqrt{\frac{2\left\|z\right\|^{2}}{\sigma_{2}^{2}(A)+\sigma_{3}^{2}(A)}},$
(9)

where $z\in\mathbb{R}^{|E|},A\in\mathbb{R}^{3\times|E|}$ are

$\displaystyle z=\begin{bmatrix}\cdots&z_{ij}&\cdots\end{bmatrix}^{T},\quad
A=\begin{bmatrix}\cdots&\frac{a_{ij}}{\left\|a_{ij}\right\|}&\cdots\end{bmatrix},$
(10)

and
$z_{ij}=\left(\left\|b_{ij}-\hat{R}a_{ij}\right\|+\delta_{ij}\right)/\left\|a_{ij}\right\|$
for each $(i,j)\in E$. $\sigma_{1}(A)\geq\sigma_{2}(A)\geq\sigma_{3}(A)$ are
the singular values of $A$.

###### Proof.

Adapted from [25, Thm. 36]. See appendix B-B. ∎

######  Remark 1.

The proof of Lemma 2 does not depend on the algorithm used to obtain
$\hat{R}$, but only on the bounds in (1). Thus Lemma 2 is applicable even with
alternative algorithms.

We provide some intuition for (9). First, $\epsilon_{R}^{2}$ is proportional
to $\sum z_{ij}^{2}$. This term captures the both the re-projection error
$\left\|b_{ij}-\hat{R}a_{ij}\right\|$, and the assumed noise bound
$\delta_{ij}$. Second, $\epsilon_{R}^{2}$ is inversely proportional to
$\sigma_{2}^{2}(A)+\sigma_{3}^{2}(A)$. These singular values characterize the
distribution of points - if the points all lie in a single line,
$\sigma_{2}(A)=\sigma_{3}(A)=0$ and $\epsilon_{R}$ tends to infinity. This
corresponds to a case where there is not enough information in the pointclouds
to determine the rotation.

While the bound in Lemma 2 is correct, we can further tighten the bounds.
Consider how the error bound scales with $|E|$. To first order, the numerator
$\left\|z\right\|^{2}$ scales with $|E|$. The denominator contains singular
values of an $3\times|E|$ matrix. The singular values will be larger when the
columns of $A$ are orthogonal to each other, and therefore, only a small
subset of the columns of $A$ contribute to large singular values. Thus we can
achieve tighter error bounds if a small subgraph of $G$ is used. This leads to
the following Lemma.

######  Lemma 3.

Consider the setup in Lemma 2. The maximum rotation error is also bounded by

$\displaystyle\left\|R-\hat{R}\right\|_{F}\leq\epsilon_{R}=\sqrt{\frac{2\left\|\tilde{z}\right\|^{2}}{\sigma_{2}^{2}\left(\tilde{A}\right)+\sigma_{3}^{2}\left(\tilde{A}\right)}}$
(11)

where $\tilde{z}\in\mathbb{R}^{3}$ and $\tilde{A}\in\mathbb{R}^{3\times 3}$
are defined as

$\displaystyle\tilde{z}=\begin{bmatrix}z_{ij}&z_{ik}&z_{il}\end{bmatrix}^{T},\quad\tilde{A}=\begin{bmatrix}\frac{a_{ij}}{\left\|a_{ij}\right\|}&\frac{a_{ik}}{\left\|a_{ik}\right\|}&\frac{a_{il}}{\left\|a_{il}\right\|}\end{bmatrix},$
(12)

where $(i,j),(i,k),(i,l)$ are any three edges selected from graph $G$.

###### Proof.

Consider a subgraph of $G$ consisting of only vertices $\\{i,j,k,l\\}$, and
edges $(i,j),(i,k),(i,l)$. Using the same rotation estimate $\hat{R}$, apply
Lemma 2. This leads to (11). ∎

Naturally, the edges should be selected to minimize $\epsilon_{R}$. This is an
NP-hard subset selection problem [31, 32]. Although heuristic methods exist,
they require constructing $A$ in (10) and computing its SVD. This alone takes
over 10 ms. Instead, we avoid constructing $A$, randomly select four nodes of
$G$, and compute $\epsilon_{R}$ using Lemma 3. We repeat this for some number
of iterations, and use the tightest bound calculated. As demonstrated in
Figure 2, as the number of iterations increases, the error bound gets tighter.
In our implementations we perform 1000 iterations. This takes under a
millisecond, and still yields reasonably tight bounds.

Figure 2: Rotation error bounds due to Lemma 3 (orange) are tighter than due
to Lemma 2 (blue). Each line shows the median and interquartile range of 100
trials of rototranslation estimation on synthetic data.

###  IV-B Translation Estimation

Once 3 is solved, we can solve for the translation:

######  Problem 4.

Consider the setup in 2. Suppose $\hat{R}\in\mathbb{SO}(3)$ solves 3.
Determine $\hat{t}\in\mathbb{R}^{3}$ that solves

$\displaystyle\underset{\begin{subarray}{c}\hat{t}\in\mathbb{R}^{3}\end{subarray}}{\text{argmin}}\sum_{i=1}^{N}\min\left(\left\|b_{i}-\hat{R}a_{i}-\hat{t}\right\|^{2},\delta_{i}^{2}\right).$
(13)

As before, the TLS is solved using GNC, where the corresponding WLS problem is
a standard least-squares problem. The following establishes an error bound.

######  Lemma 4.

Consider the setup in Problem 2. Let $\hat{R}\in\mathbb{SO}(3),\hat{t}\in
R^{3}$ solve Problem 3 and 4. Then the translation error is bounded by

$\displaystyle\left\|t-\hat{t}\right\|\leq\epsilon_{t}=\min_{i\in\\{1,...,N\\}}\left(\epsilon_{R}\left\|a_{i}\right\|+\left\|\phi_{i}\right\|+\delta_{i}\right)$
(14)

where $\phi_{i}=b_{i}-\hat{R}a_{i}-\hat{t}$ and $\epsilon_{R}$ is defined in
(9).

###### Proof.

This bound is obtained using the triangle inequality. See appendix B-C. ∎

To summarize, the C-VO algorithm computes the rototranslation between
successive frames by decomposing it into separate rotation and translation
problems. The corresponding error bounds are obtained in Lemma 3 and 4.

######  Remark 2.

The main difference of this work wrt to [25] is that we use GNC for
rototranslation estimation. GNC is robust to a small number of outliers, and
recovers the global optimal in the absence of outliers [30]. The QCQP
verification in [25] could be used, but is prohibitively slow for realtime VO.
Furthermore, it cannot handle motion blur.

Lemma 1 and 2 were reported in [25]. Lemma 3 and Lemma 4 are new results that
provide computationally efficient methods to obtain tighter bounds.

##  V Theory: Certified Mapping

This section describes our mapping algorithm. The challenge is to account for
the drift in localization from a VO algorithm, in practice often ignored by
mapping algorithms. Since we do not make this simplifying assumption, we
review ESDFs using the notation of this paper.

###  V-A Background

Let $I$ be an inertial frame. Let $\mathcal{O}\subset\mathbb{R}^{3}$ be the
set of obstacles (closed with no isolated points), assumed stationary in $I$.
$B_{k}$ is the robot body frame when the $k$-th image is received. The mapping
frame $M$ is (ideally) stationary relative to $I$, but due to VO drift, $M$
shifts relative to $I$.

The ESDF is defined as the function $d:\mathbb{R}^{3}\to\mathbb{R}$,

$\displaystyle d(p)=\min_{o\in\mathcal{O}}\left\|o-p\right\|,$ (15)

the distance between the point $p$ and the nearest obstacle. To evaluate (15),
$o$ and $p$ must be expressed in a common frame. Recall $p|^{A}$ denotes $p$
expressed in frame $A$. To represent the ESDF computationally, we discretize
the environment into a grid of voxels, and store the ESDF at each voxel. Since
this is done in the mapping frame, it is denoted
$d_{M}:\mathbb{R}^{3}\to\mathbb{R}$.

For safety-critical path planning and control, we need the ESDF at body-fixed
positions. The common approach is to assume the odometry is exact, and
determine $d(p|^{B_{k}})$ by expressing it in the map frame and evaluating
$d_{M}$:

$\displaystyle d(p|^{B_{k}})\approx
d_{M}(\hat{p}|^{M})=d_{M}(\underbrace{\hat{R}_{B_{k}}^{M}p|^{B_{k}}+\hat{t}_{B_{k}}^{M}}_{\hat{p}|^{M}})$
(16)

However, since the estimate $(\hat{R}_{B_{k}}^{M},\hat{t}_{B_{k}}^{M})$ is
inexact, this method can lead to over- or underestimates. Overestimated
distances are unsafe since they could lead to collisions.

###  V-B Proposed Approach

The goal is to construct an ESDF that is safe, i.e., underestimates the
distance to obstacles. More precisely,

######  Definition 1.

Consider an environment with obstacles $\mathcal{O}\subset\mathbb{R}^{3}$,
assumed static in frame $I$. Let the ESDF of $\mathcal{O}$ be
$d:\mathbb{R}^{3}\to\mathbb{R}$. A _Certified-ESDF_ (C-ESDF) at timestep $k$
is a function $d_{M}^{k}:\mathbb{R}^{3}\to\mathbb{R}$, such that for all
points $p|^{B_{k}}\in\mathbb{R}^{3}$,

$\displaystyle d(p|^{B_{k}})$ $\displaystyle\geq
d_{M}^{k}(\hat{p}|^{M})=d_{M}^{k}(\hat{R}_{B_{k}}^{M}p|^{B_{k}}+\hat{t}_{B_{k}}^{M})$
(17)

where $(\hat{R}_{B_{k}}^{M},\hat{t}_{B_{k}}^{M})\in\mathbb{SE}(3)$ is the
estimated rototranslation between $B_{k}$ and $M$.

Comparing (16) with (17), the goal of certification is to change the $\approx$
into $\geq$. That is, a Certified-ESDF is one where for any body-fixed point
$p|^{B_{k}}$, if the point is expressed in the mapping frame _using the
estimated rototranslation_ , we have _underestimated_ the distance to the
nearest obstacle:

$\displaystyle\underbrace{d(p|^{B_{k}})=\min_{o\in\mathcal{O}}\left\|p|^{B_{k}}-o|^{B_{k}}\right\|}_{\text{true
SDF}}$
$\displaystyle\geq\underbrace{d_{M}(\hat{R}_{B_{k}}^{M}p|^{B_{k}}+\hat{t}_{B_{k}}^{M})}_{\text{estimated
SDF}}.$ (18)

To accomplish this, we propose a strategy of deflating the ESDF. We derive a
recursive guarantee to ensure the ESDF remains certified for all $k$. Note,
Theorem 1 only describes the deflation step: see Section VI for the complete
algorithm. We start with the following assumption:

######  Assumption 1.

Assume the rototranslation error between successive timesteps is bounded by
known bounds

$\displaystyle\left\|R_{B_{k}}^{B_{k+1}}-\hat{R}_{B_{k}}^{B_{k+1}}\right\|_{F}\leq\epsilon_{R,k},\quad\left\|t_{B_{k}}^{B_{k+1}}-\hat{t}_{B_{k}}^{B_{k+1}}\right\|\leq\epsilon_{t,k}.$

Notice that 1 is exactly the result obtained in the C-VO algorithm,
specifically Lemma 3 and Lemma 4.

Consider the following update rule to construct the $(k+1)$-th ESDF from the
$k$-th ESDF:

$\displaystyle
d_{M}^{k+1}(p|^{M})=d_{M}^{k}(p|^{M})-\underbrace{\left(\epsilon_{R,k}\left\|\hat{p}|^{B_{k+1}}-\hat{t}_{B_{k}}^{B_{k+1}}\right\|+\epsilon_{t,k}\right)}_{\Delta}$
(19)

for all $p|^{M}\in\mathbb{R}^{3}$, where
$\hat{p}|^{B_{k+1}}=\hat{R}_{M}^{B_{k+1}}p|^{M}+\hat{t}_{M}^{B_{k+1}}$. Notice
the correction to C-ESDF, $\Delta$, is different at each $p|^{M}$.

Theorem 1 certifies the new ESDF:

######  Theorem 1.

Let 1 hold. Suppose at timestep $k\in\mathbb{N}$ the ESDF
$d_{M}^{k}:\mathbb{R}^{3}\to\mathbb{R}$ is a Certified-ESDF by Definition 1,
i.e., $\forall p|^{B_{k}}\in\mathbb{R}^{3}$

$\displaystyle d(p|^{B_{k}})\geq
d_{M}^{k}(\underbrace{\hat{R}_{B_{k}}^{M}p|^{B_{k}}+\hat{t}_{B_{k}}^{M}}_{p|^{B_{k}}\text{
expressed in }M}).$ (20)

Then, if the ($k+1$)-th ESDF is constructed using (19), the $(k+1)$-th ESDF is
a Certified-ESDF. That is, $\forall p|^{B_{k+1}}\in\mathbb{R}^{3}$

$\displaystyle d(p|^{B_{k+1}})\geq
d_{M}^{k+1}(\underbrace{\hat{R}_{B_{k+1}}^{M}p|^{B_{k+1}}+\hat{t}_{B_{k+1}}^{M}}_{p|^{B_{k+1}}\text{
expressed in }M}).$ (21)

###### Proof of Theorem 1.

A condensed proof is provided here. See Appendix B-D for more details.
Consider any point $p|^{B_{k+1}}\in\mathbb{R}^{3}$. When expressed in frame
$B_{k}$, we have

$\displaystyle\left\|p|^{B_{k}}-\hat{p}|^{B_{k}}\right\|$
$\displaystyle=\left\|\left(R_{B_{k+1}}^{B_{k}}p|^{B_{k+1}}+t_{B_{k+1}}^{B_{k}}\right)-\left(\hat{R}_{B_{k+1}}^{B_{k}}p|^{B_{k+1}}+\hat{t}_{B_{k+1}}^{B_{k}}\right)\right\|$
$\displaystyle=\Big{\|}\left(R_{B_{k+1}}^{B_{k}}-\hat{R}_{B_{k+1}}^{B_{k}}\right)\left(p|^{B_{k+1}}-\hat{t}_{B_{k}}^{B_{k+1}}\right)$
$\displaystyle\quad\quad+R_{B_{k+1}}^{B_{k}}\left(-t_{B_{k}}^{B_{k+1}}+\hat{t}_{B_{k}}^{B_{k+1}}\right)\Big{\|}$

by adding and subtracting $R_{B_{k+1}}^{B_{k}}\hat{t}_{B_{k}}^{B_{k+1}}$
inside the norm. Using the triangle inequality,

$\displaystyle\left\|p|^{B_{k}}-\hat{p}|^{B_{k}}\right\|$
$\displaystyle\leq\left\|R_{B_{k+1}}^{B_{k}}-\hat{R}_{B_{k+1}}^{B_{k}}\right\|\left\|p|^{B_{k+1}}-\hat{t}_{B_{k}}^{B_{k+1}}\right\|+\left\|t_{B_{k}}^{B_{k+1}}-\hat{t}_{B_{k}}^{B_{k+1}}\right\|$
$\displaystyle\leq\epsilon_{R,k}\left\|p|^{B_{k+1}}-\hat{t}_{B_{k}}^{B_{k+1}}\right\|+\epsilon_{t,k}$

Thus, when $p|^{B_{k+1}}$ is expressed in $B_{k}$, it could correspond to any
point in
$\mathcal{P}=\\{\tilde{p}\in\mathbb{R}^{3}:\left\|\tilde{p}-\hat{p}|^{B_{k}}\right\|\leq\Delta\\}$
where
$\Delta=\epsilon_{R,k}\left\|p|^{B_{k+1}}-\hat{t}_{B_{k}}^{B_{k+1}}\right\|+\epsilon_{t,k}$,
and
$\hat{p}|^{B_{k}}=\hat{R}_{B_{k+1}}^{B_{k}}p|^{B_{k+1}}+\hat{t}_{B_{k+1}}^{B_{k}}$.
Then, using properties of an SDF,

$\displaystyle d(p|^{B_{k+1}})$
$\displaystyle\geq\min_{\tilde{p}\in\mathcal{P}}d(\tilde{p})\geq
d(\hat{p}|^{B_{k}})-\Delta\geq d_{M}^{k}(\hat{p}|^{M})-\Delta$

where $\hat{p}|^{M}=\hat{R}_{B_{k}}^{M}\hat{p}|^{B_{k}}+\hat{t}_{B_{k}}^{M}$.
Therefore, we have

$\displaystyle d(p|^{B_{k+1}})$ $\displaystyle\geq
d_{M}^{k}(\hat{p}|^{M})-\epsilon_{R,k}\left\|p|^{B_{k+1}}-\hat{t}_{B_{k}}^{B_{k+1}}\right\|-\epsilon_{t,k}$
$\displaystyle=d_{M}^{k+1}(\hat{p}|^{M})$

completing the proof. ∎

Figure 3 demonstrates Theorem 1. Consider a static obstacle 1 m away from the
robot at the initial time. Suppose the robot moves by 0.1 m between frames
$k=0$ and $1$. Suppose we need the ESDF at a body-fixed point
$p|^{B_{1}}=[0.05,0.4]^{T}$. The true ESDF is thus $0.1+0.4=0.5$ m. However,
suppose the estimated rototranslation was not exact, with a 5∘ rotation error
and a 2 cm position error, as depicted on the right.

In the approximate ESDF approach (16), we would express $p|^{B_{1}}$ in frame
$M$, and compute the ESDF to be 0.526 m, greater than the true ESDF. The
approximate approach yields an unsafe estimate of the ESDF.

On the other hand, consider the proposed approach, i.e., (19). Given the
error, we have $\epsilon_{R}=0.123$ (Lemma 5) and $\epsilon_{t}=0.02$. Using
(19) the correction is $\Delta=0.070$ m. Computing the corrected ESDF yields
$d_{M}^{1}(\hat{p}|^{M})=0.456$ m. Since $0.456\leq 0.5$, this C-ESDF is an
underestimate of the true ESDF. This demonstrates safe behavior that satisfies
our definition of a Certified ESDF.

Figure 3: Diagram demonstrating the C-ESDF approach. Suppose the robot moves
by 0.1 m in the y-axis, but the estimated rototranslation has some error. The
approximate approach (16) is unsafe, estimating the ESDF of $p$ to be 0.526 m,
greater than the true ESDF of 0.5 m. In the proposed approach (19), we
calculate a correction $\Delta$, and compute the C-ESDF to be 0.456 m, a safe
underestimate.

######  Remark 3.

The form of (19) can be interpreted as follows. The update to the ESDF is
always a correction:

$\displaystyle\underbrace{d_{M}^{k+1}(p)}_{\text{new
ESDF}}=\underbrace{d_{M}^{k}(p)}_{\text{previous
ESDF}}\underbrace{-\epsilon_{R,k}\left\|\hat{p}|^{B_{k+1}}-\hat{t}_{B_{k}}^{B_{k+1}}\right\|}_{\text{rotation
correction}}\underbrace{-\epsilon_{t,k}}_{\text{translation correction}}$

With a non-zero translation error the ESDF must be decremented everywhere by
$\epsilon_{t}$. With a non-zero rotation error, every point must also be
decremented, but the decrement increases with the distance from the camera
origin: a 2∘ rotation error leads to a $2\pi/180\approx 0.03$ m error for a
point 1 m away, but a 0.3 m error for a point 10 m away.

##  VI Algorithms and Implementation Details

Here we use the theoretical results of Section IV and V to construct
algorithms for 1. Our implementations are open-sourced at
https://github.com/dasc-lab/certified-perception.

###  VI-A C-VO

The first step is to identify and track features in successive RGBD images. We
use Good Features to Track [28], and the Lukas-Kanade optical flow algorithm
[29], since these represent well established baseline methods. By performing
the feature detection and mapping in the 2D image space, we avoid the
complexities of feature selection and matching in 3D pointclouds [33, 34]. In
our experiments, we observe approximately 300 features matched per frame and a
1-5% outlier rate, depending on motion blur, lighting conditions, and the
richness of the observed scene.

At 300 features per frame, a complete graph $G$ would have $|E|=44,850$ edges.
This is computational bottleneck in (7b). Empirically, we observe that only a
small fraction $f\in(0,1)$ of edges are needed to recover the true solution of
the rotation estimation problem Figure 4. The figure shows that as $f$
reduces, the computation time decreases, but the accuracy is largely
unaffected. We use $f=5$%, as a compromise between runtime, accuracy, and
robustness.

Figure 4: Effect of graph fraction on accuracy and runtime of Certified Visual
Odometry (CVO). (a) Log-Log plot of the computation time. (b) Log-Linear plot
of accuracy. Both graphs were produced by running the algorithm on a recorderd
dataset using the Realsense D455 camera, and ground-truth provided by VICON.

1 When  _received $(k+1)$-th RGBD image_

2 Using optical flow, identify corresponding features in RGB frames $k$ and
$k+1$.

3 Project features to 3D using the depth images.

4 Construct graph $G$ connecting pairs of features.

5 Determine $\hat{R}_{k}^{k+1},\hat{t}_{k}^{k+1}$ using (6), (13) and GNC.

6 Determine $\epsilon_{R,k},\epsilon_{t,k}$ using (11), (14).

7 Publish
$(\hat{R}_{k}^{k+1},\hat{t}_{k}^{k+1},\epsilon_{R,k},\epsilon_{t,k})$.

8

Algorithm 1 Certified Visual Odometry

###  VI-B C-ESDF

Our implementation of the C-ESDF builds on [7]. We describe our modifications,
assuming familiarity with [6, 7].

As in [7], we maintain two SDFs, the Truncated Signed Distance Field (TSDF)
and the ESDF. The TSDF only contains values near the obstacle geometry, and is
used to determine the zero level-set of the obstacles. When a new depth images
are received, we use raycasting to update the TSDF of voxels within the FoV.
From the updated cells, we propagate waves to construct the ESDF.

Our modification is to add a deflation step, based on Theorem 1. We keep track
of a correction in each voxel, the cumulative sum of decrements. When a voxel
lies within the FoV, the correction is reset to zero, allowing new regions to
be added to the free set. When publishing the C-ESDF, we subtract the
correction from estimated ESDF. Since the decrement at each voxel is
independent of every other voxel, the decrement step can be parallelized
efficiently on the GPU.

1 When  _received $(k+1)$-th RGB-D image_

2 Determine $(\hat{R}_{B_{k}}^{B_{k+1}},\hat{t}_{B_{k}}^{B_{k+1}})$ and
$\epsilon_{R,k},\epsilon_{t,k}$ using Algorithm 1.

3 Update the TSDF within camera’s FOV.

4 Update correction of all voxels using Thm. 1.

5 For all voxels in FOV, reset correction to 0.

6

7Every  _$T$ seconds_

8 Propagate waves from all surface voxels to update the C-ESDF.

9

Algorithm 2 Certified Mapping

##  VII Experimental Evaluation

Here we report our experiments. As part of developing these libraries,
extensive tests were performed on synthetic data and test datasets, but
omitted in the interest of space. Only results from hardware experiments are
reported.

###  VII-A C-VO

Figure 5: Comparison of the Certified Visual Odometry (C-VO) algorithm with benchmark algorithms. We compare C-VO with Nvidia VSlam [35] and VINS-Fusion [13]. To compute the error, we also recorded the ground-truth trajectory using a VICON motion capture system. To enable a fair comparison, each method was run in Visual Odometry only mode, i.e., without loop-closures or IMU measurements. (a) The reconstructed trajectory from each method. (b-g) The relative rotation error and relative translation error for each method. Only our method provides error bounds. TABLE I: Comparison of C-VO with state-of-the-art algorithms.  Method | RMS Rotation Error (-) | RMS Translation Error (cm) | Time / iteration (ms)
---|---|---|---
V-SLAM [35] | 0.00376 | 0.242 | 1.40
VINS-Fusion [13] | 0.03941 | 3.102 | 39.24
C-VO [ours] | 0.00380 | 0.882 | 14.24
C-VO [ours] Error Bound | 0.03177 | 7.239 | (included above)

Figure 5 and Table I compare the performance of the C-VO method to two state-
of-the-art methods for Visual Odometry, Nvidia VSLAM [35] and VINS-Fusion
[13]. Note, for a fair comparison, we turn off the IMU and loop-closure
components of each algorithm. We use VICON for ground-truth. The following
metrics measure the relative rototranslation error,555These definitions differ
to those in [36], since these correspond to $\epsilon_{R},\epsilon_{t}$.

$\displaystyle RRE_{k}$
$\displaystyle=\left\|\hat{R}_{B_{k}}^{B_{k+1}}-R_{B_{k}}^{B_{k+1}}\right\|_{F},$
$\displaystyle RTE_{k}$
$\displaystyle=\left\|\hat{t}_{B_{k}}^{B_{k+1}}-t_{B_{k}}^{B_{k+1}}\right\|,$

where the quantities with and without $\hat{(\cdot)}$ denote the estimates and
the ground truths respectively.

The results show that our C-VO algorithm produces an rotation error that is
on-par with VSLAM, but accumulates 6mm of additional translation error per
frame (in terms of the RMSE). Our algorithm is more accurate than VINS-Fusion,
with an order of magnitude lower rotation error, and a third of the
translation error (Table I). Table I also shows that VSLAM implementation is
10x faster than ours, and 30x faster than VINS-Fusion, likely due to
specialized GPU code. This makes the entire (closed-source) VSLAM odometry
algorithm faster than the OpenCV feature detection step alone. Improving our
implementation should allow us to track more features and therefore improve
performance.

The primary benefit of our algorithm is that it is able to produce error
bounds, plotted in Figs. 5d, 5g. The error bound is always greater than the
measured error. The error bound is (in terms of the RMSE) about an order of
magnitude greater than the measured error. This is to be expected, since our
error bounds are calculated assuming worst case disturbances, and extensive
use of the triangle inequality.

###  VII-B C-ESDF

Figure 6: Certified Mapping. (a-c) Snapshots of the ESDF using the baseline
algorithm. (d-f) Snapshots using the proposed C-ESDF algorithm. (g, h) Insets
showing (c, f) magnified. Each figure shows a slice at $z=1.0$ m of the
constructed 3D ESDF. The figures show that while the baseline ESDF produces a
map of entire lab space, while the C-ESDF produces a local map. However, due
to the accumulated error, the baseline ESDF misclassified some regions to be
free of obstacles, while the C-ESDF does not have this issue.

Figure 6 shows the performance of the Certified ESDF algorithm (bottom row),
compared to the baseline implementation from Nvidia [7] (top row). The results
are easier to understand in the supplementary video, at
https://github.com/dasc-lab/certified-perception.

In all figures we show the scene in the mapping frame. Due to VO drift the
obstacles are not stationary in the mapping frame. Using VICON we plot the
ground-truth location of the obstacles relative to the camera. The objective
is to build a map where for all voxels inside the blue shapes, the map is
either unknown (white) or occupied (red), but not free (yellow).

Initially the maps generated are the same (Figs. 6a, 6d). Between $0$ and $5$
s, the camera yaws by 90∘, and starts seeing a new region (Figs. 6b, 6e). In
the baseline (Fig. 6b) the region outside the camera’s FoV is not updated. In
the C-ESDF, since the left region is unobserved, the deflation step decrements
the ESDF, and some cells are marked red (Fig. 6e).

After $60$ s, a large portion of the lab has now been mapped (Figs. 6c, 6f).
The accumulated VO drift leads to errors in the baseline map. In particular,
consider the magnified insets (Figs. 6g, 6h). In Fig. 6g we see some yellow
cells that should be red. In contrast, in Fig. 6h, the obstacles are marked
correctly.666Note, there are some cells in the C-ESDF that are still
incorrect, due to numerical issues and the resolution used. Notice the
forgotten regions are not based on either the distance of the point to the
origin of the camera, or the time that elapsed since the point was in the
camera’s FoV. These are two common heuristics used, but the C-VO algorithm
deflates the ESDF based on the actual odometry drift.

Overall, we see that with the C-ESDF algorithm, the region of the map that is
classified as either free or obstacle is smaller (i.e., greater parts of the
map are unknown). However, the regions that are marked as free are indeed free
relative to the ground truth obstacle geometry. Furthermore, with more
accurate C-VO, the deflation rate will be smaller.

###  VII-C Timing Analysis

TABLE II: Timing Breakdown. All steps except for the ESDF wave propagation happen at each new camera frame, i.e., 30 Hz. The wave propagation occurs at 5 Hz. Module | Step | Mean Time (ms)
---|---|---
C-VO | Feature Detection | 2.804
| Optical Flow | 0.935
| Estimate Rototranslation | 1.463
| Compute Error Bounds | 2.365
C-ESDF | Integrate Depth into TSDF | 0.674
| Deflate ESDF | 0.282
| ESDF Wave Propagation* | 5.491

Table II breaks down the computation time for major steps of C-VO and C-ESDF.
The cameras are operating at 30FPS, i.e., all processing must occur within 33
ms. We see that the C-VO algorithm takes about 7 ms, of which half is the
feature detection and optical flow. The C-ESDF algorithm is fast, taking about
1 ms to integrate a new depth image and to deflate the ESDF. By distributing
the deflation step over the GPU, the deflation step can be performed very
efficiently. The proposed methods can run in realtime.

All reported results are from experiments run on a Ryzen7 5800h 16GB, with a
3050Ti. We have performed similar experiments on the Xavier NX 16 GB. On the
Xavier, the C-VO module takes approximately 18 ms, and therefore can still run
in realtime. The computation time depends heavily on the number of features
observed in each frame, and therefore the variance of the compute time can be
significant.

###  VII-D Limitations and Future Work

There are a few directions in which this work can be extended. (A)
Improvements in feature detection and tracking in the RGB images are required
for the C-VO algorithm to be reliable when executing aggressive maneuvers. (B)
Incorporating information from an IMU or keeping track of multiple sets of
pointclouds could significantly improve the robustness of the C-VO algorithm.
(C) A semantic or geometric segmentation of the received images could be used
to mask dynamic obstacles from the environment. It could also be used in the
C-ESDF, as the deflation correction around dynamic obstalces could be
increased based on the maximum speed of the obstacles. (D) The C-VO error
bounds depend explicitly on the singular values of $A$ in (9). Using the ESDF,
one could predict the regions of the map that lack features and avoid them in
the path planner. This can help keep the C-VO error small.

##  VIII Conclusion

This paper has taken an initial step towards building certified perception
algorithms applicable to safety critical planning and control. Our goal was to
use the visual information from an RGBD camera, construct a pose estimate, and
build a map of the obstacle geometry. The generated map needs to be correct in
a body-fixed frame, i.e., to ensure that regions marked as free are indeed
free relative to the robot.

To achieve this, we first developed a Certified Visual Odometry (C-VO)
algorithm. This casts visual odometry as a pointcloud registration problem. We
propose a robust truncated least squares algorithm to solve for the rotation
and the translation between successive camera frames, and use the geometric
properties to derive an error bound on rototranslation estimate.

The second step was to devise a Certified Mapping algorithm. We used the C-VO
error bound to deflate the signed distance field representing the world. By
choosing to deflate safe regions (instead of inflating known obstacles) we
allow the environment to be continuously explored, and allow new information
from sensors to be assimilated into the map. The decrement in the signed
distance field at each point of the map is, again, derived using on geometric
properties.

Importantly, the C-VO and C-ESDF algorithms integrate naturally. The error
bound on C-VO takes the form of a norm ball, and the same norm ball is used to
deflate the ESDF.

Finally, we have experimental demonstrations of both the C-VO and C-ESDF
algorithms indicate their ability to run in realtime, although they can still
be made more efficient.

## References

  * [1] A. D. Ames, X. Xu, J. W. Grizzle, and P. Tabuada, “Control barrier function based quadratic programs for safety critical systems,” _IEEE TAC_ , vol. 62, no. 8, pp. 3861–3876, 2016.
  * [2] K. Garg, J. Usevitch, J. Breeden, M. Black, D. Agrawal, H. Parwana, and D. Panagou, “Advances in the theory of control barrier functions: Addressing practical challenges in safe control synthesis for autonomous and robotic systems,” _arXiv preprint arXiv:2312.16719_ , 2023.
  * [3] B. T. Lopez and J. P. How, “Aggressive 3-D collision avoidance for high-speed navigation.” in _IEEE ICRA_ , 2017, pp. 5759–5765.
  * [4] J. Tordesillas, B. T. Lopez, and J. P. How, “Faster: Fast and safe trajectory planner for flights in unknown environments,” in _IEEE IROS_. IEEE, 2019, pp. 1934–1940.
  * [5] D. Agrawal, R. Chen, and D. Panagou, “gatekeeper: Online safety verification and control for nonlinear systems in dynamic environments,” in _IEEE IROS_. IEEE, 2023, pp. 259–266.
  * [6] H. Oleynikova, Z. Taylor, M. Fehr, R. Siegwart, and J. Nieto, “Voxblox: Incremental 3d euclidean signed distance fields for on-board mav planning,” in _IEEE IROS_. IEEE, 2017, pp. 1366–1373.
  * [7] Nvidia-Isaac, “NvBlox,” https://github.com/nvidia-isaac/nvblox, 2023\.
  * [8] A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, and W. Burgard, “Octomap: An efficient probabilistic 3d mapping framework based on octrees,” _Autonomous robots_ , vol. 34, pp. 189–206, 2013.
  * [9] A. Rosinol, J. J. Leonard, and L. Carlone, “Nerf-slam: Real-time dense monocular slam with neural radiance fields,” in _IEEE ICRA_. IEEE, 2023, pp. 3437–3444.
  * [10] D. M. Rosen, L. Carlone, A. S. Bandeira, and J. J. Leonard, “SE-Sync: a certifiably correct algorithm for synchronization over the special euclidean group,” _IEEE IJRR_ , vol. 38, no. 2-3, pp. 95–125, 2019.
  * [11] M. Marchi, J. Bunton, B. Gharesifard, and P. Tabuada, “LiDAR point cloud registration with formal guarantees,” in _IEEE CDC_ , 2022, pp. 3462–3467.
  * [12] D. Scaramuzza and F. Fraundorfer, “Visual odometry [tutorial],” _IEEE Robot. & Automat. Mag._, vol. 18, no. 4, pp. 80–92, 2011.
  * [13] Z. Yu, L. Zhu, and G. Lu, “Vins-motion: tightly-coupled fusion of vins and motion constraint,” in _IEEE ICRA_. IEEE, 2021, pp. 7672–7678.
  * [14] K. Chen, R. Nemiroff, and B. T. Lopez, “Direct lidar-inertial odometry: Lightweight lio with continuous-time motion correction,” in _IEEE ICRA_. IEEE, 2023, pp. 3983–3989.
  * [15] Y. Tian, Y. Chang, F. H. Arias, C. Nieto-Granda, J. P. How, and L. Carlone, “Kimera-multi: Robust, distributed, dense metric-semantic slam for multi-robot systems,” _IEEE TRO_ , vol. 38, no. 4, 2022.
  * [16] S. Kolathaya and A. D. Ames, “Input-to-state safety with control barrier functions,” _IEEE LCSS_ , vol. 3, no. 1, pp. 108–113, 2018.
  * [17] M. Black, G. Fainekos, B. Hoxha, D. Prokhorov, and D. Panagou, “Safety under uncertainty: Tight bounds with risk-aware control barrier functions,” in _IEEE ICRA_ , 2023, pp. 12 686–12 692.
  * [18] P. Culbertson, R. K. Cosner, M. Tucker, and A. D. Ames, “Input-to-state stability in probability,” _arXiv preprint arXiv:2304.14578_ , 2023.
  * [19] S. Dean, A. Taylor, R. Cosner, B. Recht, and A. Ames, “Guaranteeing safety of learned perception modules via measurement-robust control barrier functions,” in _IEEE CORL_. PMLR, 2021, pp. 654–670.
  * [20] D. R. Agrawal and D. Panagou, “Safe and robust observer-controller synthesis using control barrier functions,” _IEEE LCSS_ , vol. 7, pp. 127–132, 2022\.
  * [21] T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatile monocular visual-inertial state estimator,” _IEEE TRO_ , vol. 34, no. 4, pp. 1004–1020, 2018.
  * [22] K. Mohta, K. Sun, S. Liu, M. Watterson, B. Pfrommer, J. Svacha, Y. Mulgaonkar, C. J. Taylor, and V. Kumar, “Experiments in fast, autonomous, gps-denied quadrotor flight,” in _IEEE ICRA_. IEEE, 2018, pp. 7832–7839.
  * [23] “Intel Realsense D455 Tech Specs,” Jan 2024. [Online]. Available: https://www.intelrealsense.com/depth-camera-d455/
  * [24] C. V. Nguyen, S. Izadi, and D. Lovell, “Modeling kinect sensor noise for improved 3d reconstruction and tracking,” in _Intl. Conf. 3D imaging, modeling, processing, visualization & transmission_. IEEE, 2012, pp. 524–530.
  * [25] H. Yang, J. Shi, and L. Carlone, “TEASER: Fast and certifiable point cloud registration,” _IEEE TRO._ , vol. 37, no. 2, pp. 314–333, 2020.
  * [26] I. Cvišić, I. Marković, and I. Petrović, “SOFT2: stereo visual odometry for road vehicles based on a point-to-epipolar-line metric,” _IEEE TRO_ , vol. 39, no. 1, pp. 273–288, 2022.
  * [27] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C. J. Taylor, and V. Kumar, “Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments,” _IEEE RAL_ , vol. 2, no. 3, pp. 1688–1695, 2017.
  * [28] J. Shi _et al._ , “Good features to track,” in _Proc. Conf. Comp. Vision and Pattern Recog._ IEEE, 1994, pp. 593–600.
  * [29] B. D. Lucas and T. Kanade, “An iterative image registration technique with an application to stereo vision,” in _IJCAI_ , vol. 2, 1981, pp. 674–679.
  * [30] H. Yang, P. Antonante, V. Tzoumas, and L. Carlone, “Graduated non-convexity for robust spatial perception: From non-minimal solvers to global outlier rejection,” _IEEE RAL_ , vol. 5, no. 2, pp. 1127–1134, 2020.
  * [31] F. De Hoog and R. Mattheij, “Subset selection for matrices,” _Linear Algebra and its Applications_ , vol. 422, no. 2-3, pp. 349–359, 2007.
  * [32] H. Avron and C. Boutsidis, “Faster subset selection for matrices and applications,” _J. Matrix Anal. Appl._ , vol. 34, no. 4, pp. 1464–1499, 2013\.
  * [33] F. Tombari, S. Salti, and L. Di Stefano, “Performance evaluation of 3d keypoint detectors,” _Intl. J. of Comp. Vision_ , vol. 102, no. 1-3, pp. 198–220, 2013.
  * [34] C. Choy, J. Park, and V. Koltun, “Fully convolutional geometric features,” in _IEEE ICCV_ , 2019, pp. 8958–8966.
  * [35] Nvidia-Isaac, “cuVSLAM,” https://nvidia-isaac-ros.github.io/concepts/visual˙slam/cuvslam/index.html, 2024\.
  * [36] M. Grupp, “evo: Python package for the evaluation of odometry and slam.” https://github.com/MichaelGrupp/evo, 2017.
  * [37] D. Q. Huynh, “Metrics for 3D rotations: Comparison and analysis,” _J. Math. Imag. Vision_ , vol. 35, pp. 155–164, 2009.

##  Appendix A Notation and Additional Symbols

TABLE III:  Symbol |  Description
---|---
$I$ |  Inertial Frame
$M$ |  Mapping Frame
$B_{k}$ |  Body Frame at timestep $k$
$p|^{F}$ |  Point $p$ expressed in frame $F$
$\mathcal{O}\subset\mathbb{R}^{3}$ |  Obstacle set
$d(p)$ |  SDF (frame-independent)
$d_{M}(p|^{M})$ |  SDF expressed in frame $M$
$d_{M}^{k}(p|^{M})$ |  SDF expressed in frame $M$ at timestep $k$
$\epsilon_{R,k}$ |  Rotation error bound between frames $B_{k}$ and $B_{k+1}$
$\epsilon_{t,k}$ |  Translation error bound between frames $B_{k}$ and $B_{k+1}$

##  Appendix B Proofs

###  B-A Proof of Lemma 1

###### Proof.

For ease of notation let $k=ij$ be used to index edge $(i,j)$ of the graph
$G$. Reformulating (6) in terms of quaternions, we have

$\displaystyle\underset{\begin{subarray}{c}q\in\mathbb{S}^{3}\end{subarray}}{\text{argmin}}\sum_{k=1}^{|E|}w_{k}\left\|\overline{b}_{k}-q\circ\overline{a}_{k}\circ
q^{-1}\right\|^{2}$

where $\overline{a}_{k}=\begin{bmatrix}a_{k}^{T}&0\end{bmatrix}^{T}$,
$\overline{b}_{k}=\begin{bmatrix}b_{k}^{T}&0\end{bmatrix}^{T}$. Since

$\displaystyle w_{k}\left\|\overline{b}_{k}-q\circ\overline{a}_{k}\circ
q^{-1}\right\|^{2}$
$\displaystyle\quad=w_{k}\left\|\overline{b}_{k}-\Omega_{2}^{T}(q)\Omega_{1}(q)\overline{a}_{k}\right\|^{2}$
$\displaystyle\quad=w_{k}((\overline{b}_{k}-\Omega_{2}^{T}(q)\Omega_{1}(q)\overline{a}_{k})^{T}(\overline{b}_{k}-\Omega_{2}^{T}(q)\Omega_{1}(q)\overline{a}_{k}))$
$\displaystyle\quad=w_{k}(\overline{b}_{k}^{T}\overline{b}_{k}-\overline{b}_{k}^{T}\Omega_{2}^{T}(q)\Omega_{1}(q)\overline{a}_{k}-\overline{a}_{k}^{T}\Omega_{1}^{T}(q)\Omega_{2}(q)\overline{b}_{k}+\overline{a}_{k}^{T}\overline{a}_{k})$
$\displaystyle\quad=w_{k}(\left\|\overline{b}_{k}\right\|^{2}-\overline{b}_{k}^{T}\Omega_{2}^{T}(q)\Omega_{1}(q)\overline{a}_{k}-\overline{a}_{k}^{T}\Omega_{1}^{T}(q)\Omega_{2}(q)\overline{b}_{k}+\left\|\overline{a}_{k}\right\|^{2})$
$\displaystyle\quad=w_{k}(\left\|\overline{b}_{k}\right\|^{2}-q^{T}\Omega_{1}^{T}(\overline{b}_{k})\Omega_{2}(\overline{a}_{k})q-q^{T}\Omega_{2}^{T}(\overline{a}_{k})\Omega_{1}(\overline{b}_{k})q+\left\|\overline{a}_{k}\right\|^{2})$
$\displaystyle\quad=w_{k}(\left\|a_{k}\right\|^{2}+\left\|b_{k}\right\|^{2})-q^{T}(w_{k}(\Omega_{1}^{T}(\overline{b}_{k})\Omega_{2}(\overline{a}_{k})+\Omega_{2}^{T}(\overline{a}_{k})\Omega_{1}(\overline{b}_{k})))q$

Since $q$ does not appear in the first term, it can be dropped from the
optimization problem, and (B-A) is equivalent to

$\displaystyle\underset{\begin{subarray}{c}q\in\mathbb{S}^{3}\end{subarray}}{\text{argmin}}\
q^{T}Qq$

where $Q\in\mathbb{R}^{4\times 4}$ is given in (7b).

Finally, by interpreting the quaternion $q\in\mathbb{S}^{3}$ as a unit vector
$q\in\mathbb{R}^{4}$, we have

$\displaystyle\underset{\begin{subarray}{c}q\in\mathbb{S}^{3}\end{subarray}}{\text{argmin}}\
q^{T}Qq=\underset{\begin{subarray}{c}q\in\mathbb{R}^{4}\end{subarray}}{\text{argmin}}\
q^{T}Qq\ \text{s.t.}\left\|q\right\|=1$

which is the canonical eigenvector problem. ∎

###  B-B Proof of Lemma 2

###### Proof.

The proof is based on [25, Thm. 37], but is repeated here in the notation of
this paper. Note, we do not consider scale errors or adversarial outliers.
Given any $(i,j)\in E$,

$\displaystyle b_{ij}=Ra_{ij}+\epsilon_{ij}$

where
$\left\|\epsilon_{ij}\right\|\leq\left\|\epsilon_{i}\right\|+\left\|\epsilon_{j}\right\|\leq\delta_{ij}=\delta_{i}+\delta_{j}$.

If we define

$\displaystyle\phi_{ij}=b_{ij}-\hat{R}a_{ij}$

then, $b_{ij}=\hat{R}a_{ij}+\phi_{ij}$. Equating the two expressions for
$b_{ij}$,

$\displaystyle(R-\hat{R})a_{ij}$ $\displaystyle=\phi_{ij}-\epsilon_{ij}$
$\displaystyle\therefore\left\|(R-\hat{R})\frac{a_{ij}}{\left\|a_{ij}\right\|}\right\|$
$\displaystyle=\frac{\left\|\phi_{ij}-\epsilon_{ij}\right\|}{\left\|a_{ij}\right\|}\leq\frac{\left\|\phi_{ij}\right\|+\delta_{ij}}{\left\|a_{ij}\right\|}$

Define $A\in\mathbb{R}^{3\times|E|}$ as the matrix obtained by horizontally
stacking each normalized $a_{ij}$:

$\displaystyle
A=\begin{bmatrix}\cdots&\frac{a_{ij}}{\left\|a_{ij}\right\|}&\cdots\end{bmatrix}$

Then, using the properties of the Frobenius norm,

$\displaystyle\left\|(R-\hat{R})A\right\|_{F}^{2}\leq\sum_{(i,j)\in
E}z_{ij}^{2}$

where $z_{ij}=(\left\|\phi_{ij}\right\|+\delta_{ij})/\left\|a_{ij}\right\|$.

The objective is to lower-bound $\left\|R-\hat{R}\right\|_{F}$. Consider the
matrix

$\displaystyle Z$ $\displaystyle=(R-\hat{R})^{T}(R-\hat{R})$
$\displaystyle=2I-(S^{T}+S)$

where $S=\hat{R}^{T}R\in\mathbb{SO}(3)$. Since $S\in\mathbb{SO}(3)$, let the
axis-angle representation of $S$ be $(s,\theta)$, where $s\in\mathbb{R}^{3}$
is a unit vector, and $\theta\in\mathbb{R}$ is the angular distance between
$R,\hat{R}$. Thus, the axis-angle representation of $S^{T}$ is $(s,-\theta)$.
Therefore, using the Rodrigues formula,

$\displaystyle S$
$\displaystyle=\cos(\theta)I+\sin(\theta)[s]_{\times}+(1-\cos\theta)ss^{T}$
$\displaystyle S^{T}$
$\displaystyle=\cos(\theta)I-\sin(\theta)[s]_{\times}+(1-\cos\theta)ss^{T}$

where $[s]_{\times}\in\mathbb{R}^{3\times 3}$ is the skew-symmetric matrix
associated with $s\in\mathbb{R}^{3}$. Inserting these equations into $Z$, we
have

$\displaystyle Z$ $\displaystyle=2(1-\cos(\theta))I-2(1-\cos\theta)ss^{T}$

Next, we investigate the eigenvalue decomposition of $Z$. Notice $s$ is an
eigenvector of $Z$, associated with an eigenvalue of $0$:

$\displaystyle Zs$ $\displaystyle=2(1-\cos(\theta))s-2(1-\cos\theta)ss^{T}s$
$\displaystyle=2(1-\cos(\theta))s-2(1-\cos\theta)s=0$

since $s$ is a unit vector, i.e., $s^{T}s=1$. Now consider any two vectors
$w_{1},w_{2}\in\mathbb{R}^{3}$ perpendicular to $s$ and each other. For either
$w\in\\{w_{1},w_{2}\\}$,

$\displaystyle Zw$ $\displaystyle=2(1-\cos(\theta))w-2(1-\cos\theta)ss^{T}w$
$\displaystyle=2(1-\cos(\theta))w$

since $s^{T}w=0$. Thus, the eigenvalues of $Z$ are $\\{\lambda,\lambda,0\\}$
where $\lambda=2(1-\cos\theta)$. Since $Z^{1/2}=(R-\hat{R})$, the singular
values of $(R-\hat{R})$ are $\\{\sqrt{\lambda},\sqrt{\lambda},0\\}$.

Now let the Singular Value Decomposition (SVD) of $(R-\hat{R})$ be $U\Sigma
V^{T}$. Then,

$\displaystyle\left\|(R-\hat{R})^{T}A\right\|_{F}^{2}=\left\|U\Sigma
V^{T}A\right\|_{F}^{2}=\left\|\Sigma V^{T}A\right\|_{F}^{2}$ $\displaystyle=$
$\displaystyle\left\|\begin{bmatrix}\sqrt{\lambda}\\\ &\sqrt{\lambda}&\\\
&&0\end{bmatrix}\begin{bmatrix}(V^{T}A)_{1}\\\ (V^{T}A)_{2}\\\
(V^{T}A)_{3}\end{bmatrix}\right\|_{F}^{2}$ $\displaystyle=$
$\displaystyle\lambda\left(\left\|(V^{T}A)_{1}\right\|^{2}+\left\|(V^{T}A)_{2}\right\|^{2}\right)$
$\displaystyle=$
$\displaystyle\lambda\left(\left\|V^{T}A\right\|_{F}^{2}-\left\|(V^{T}A)_{3}\right\|^{2}\right)$

where the notation $(V^{T}A)_{1}$ denotes the first row of $V^{T}A$. Recall
singular values of a matrix $X\in\mathbb{R}^{3\times 3}$ are listed in non-
increasing order: $\sigma_{1}(X)\geq\sigma_{2}(X)\geq\sigma_{3}(X)\geq 0$.
Since $V$ is a unitary matrix, and
$\left\|XY\right\|_{F}^{2}\leq\sigma_{1}^{2}(Y)\left\|X\right\|_{F}^{2}$ for
any two matrices $X,Y$, we have

$\displaystyle\left\|(V^{T}A)_{3}\right\|^{2}$
$\displaystyle=\left\|\begin{bmatrix}0\\\ &0\\\
&&1\end{bmatrix}V^{T}A\right\|_{F}^{2}$
$\displaystyle\leq\sigma_{1}^{2}(A)\left\|\begin{bmatrix}0\\\ &0\\\
&&1\end{bmatrix}V^{T}\right\|_{F}^{2}$
$\displaystyle\therefore\left\|(V^{T}A)_{3}\right\|^{2}$
$\displaystyle\leq\sigma_{1}^{2}(A)$

Now using the property
$\left\|V^{T}A\right\|_{F}^{2}=\left\|A\right\|_{F}^{2}=\sigma_{1}^{2}(A)+\sigma_{2}^{2}(A)+\sigma_{3}^{2}(A)$,
we have

$\displaystyle\left\|(R-\hat{R})^{T}A\right\|_{F}^{2}$
$\displaystyle=\lambda\left(\left\|V^{T}A\right\|_{F}^{2}-\left\|(V^{T}A)_{3}\right\|^{2}\right)$
$\displaystyle\geq\lambda\left(\sigma_{1}^{2}(A)+\sigma_{2}^{2}(A)+\sigma_{3}^{2}(A)-\sigma_{1}^{2}(A)\right)$
$\displaystyle\geq\lambda\left(\sigma_{2}^{2}(A)+\sigma_{3}^{2}(A)\right)$

Combining the upper and lower bounds on $\left\|(R-\hat{R})^{T}A\right\|_{F}$,

$\displaystyle\lambda(\sigma_{2}^{2}(A)+\sigma_{3}^{2}(A))$
$\displaystyle\leq\left\|(R-\hat{R})^{T}A\right\|_{F}^{2}\leq\sum_{(i,j)\in
E}z_{ij}^{2}$ $\displaystyle\therefore\lambda$
$\displaystyle\leq\frac{1}{\sigma_{2}^{2}(A)+\sigma_{3}^{2}(A)}\sum_{(i,j)\in
E}z_{ij}^{2}$

This leads to the Frobenius bound:

$\displaystyle\left\|R-\hat{R}\right\|_{F}^{2}$
$\displaystyle=\operatorname{tr}((R-\hat{R})(R-\hat{R})^{T})$
$\displaystyle=\operatorname{tr}(2I-R\hat{R}^{T}-\hat{R}R^{T})$
$\displaystyle=6-2\operatorname{tr}(R\hat{R}^{T})$
$\displaystyle=6-2(1+2\cos\theta)$ $\displaystyle=2\lambda$
$\displaystyle\leq\frac{2}{\sigma_{2}^{2}(A)+\sigma_{3}^{2}(A)}\sum_{(i,j)\in
E}z_{ij}^{2}$

since $\lambda=2(1-\cos\theta)$. ∎

######  Lemma 5.

Given $R_{1},R_{2}\in\mathbb{SO}(3)$, the error between the rotations can be
expressed equivalently as a Frobenius norm or as an angular error:

$\displaystyle\cos\theta=1-\frac{\left\|R_{1}-R_{2}\right\|_{F}^{2}}{4}.$ (22)

###### Proof.

$\displaystyle\left\|R_{1}-R_{2}\right\|_{F}^{2}$
$\displaystyle=\operatorname{tr}\left((R_{1}-R_{2})(R_{1}-R_{2})^{T}\right)$
(23)
$\displaystyle=\operatorname{tr}\left(2I-R_{1}R_{2}^{T}-R_{2}R_{1}^{T}\right)$
(24) $\displaystyle=6-2\operatorname{tr}\left(R_{1}R_{2}^{T}\right)$ (25)

Now defining the relative rotation from $R_{1}$ to $R_{2}$ as
$\Delta=R_{1}R_{2}^{T}\in\mathbb{SO}(3)$, the minimum angle corresponding to
the rotation $\Delta$ is $\theta\geq 0$ about some axis $u\in\mathbb{R}^{3}$.
Then, $\operatorname{tr}\left(\Delta\right)=1+2\cos\theta$ according to [37,
Appendix A]. Therefore,

$\displaystyle\left\|R_{1}-R_{2}\right\|_{F}^{2}$
$\displaystyle=6-2(1+2\cos\theta)$ (26) $\displaystyle=4(1-\cos\theta)$ (27)

∎

###  B-C Proof of Lemma 4

###### Proof.

Consider any $i\in\mathcal{I}$. Then,

$\displaystyle b_{i}=Ra_{i}+t+\epsilon_{i}$ $\displaystyle
b_{i}=\hat{R}a_{i}+\hat{t}+\phi_{i}.$

Therefore, equating these two expressions,

$\displaystyle t-\hat{t}$
$\displaystyle=(\hat{R}-R)a_{i}+\phi_{i}-\epsilon_{i}$
$\displaystyle\therefore\left\|t-\hat{t}\right\|$
$\displaystyle\leq\left\|(\hat{R}-R)a_{i}\right\|+\left\|\phi_{i}\right\|+\left\|\epsilon_{i}\right\|$
$\displaystyle\leq\left\|\hat{R}-R\right\|_{2}\left\|a_{i}\right\|+\left\|\phi_{i}\right\|+\delta_{i}$
$\displaystyle\leq\left\|\hat{R}-R\right\|_{F}\left\|a_{i}\right\|+\left\|\phi_{i}\right\|+\delta_{i}$
$\displaystyle\leq\epsilon_{R}\left\|a_{i}\right\|+\left\|\phi_{i}\right\|+\delta_{i}$

Since this holds true for all $i\in\mathcal{I}$, we take the minimum of the
RHS over $i$, yielding the expression in the theorem statement. ∎

######  Lemma 6.

For any rototranslation $(R_{A}^{B},t_{A}^{B})\in\mathbb{SE}(3)$,

$\displaystyle t_{B}^{A}=-R_{B}^{A}t_{A}^{B}.$

###### Proof.

Consider some vector $v|^{B}$. Then

$\displaystyle v|^{A}=R_{B}^{A}v|^{B}+t_{B}^{A}$ $\displaystyle\therefore
t_{B}^{A}=v|^{A}-R_{B}^{A}v|^{B}$

Furthermore, $v|^{B}=R_{A}^{B}v|^{A}+t_{A}^{B}$. Therefore,

$\displaystyle t_{B}^{A}$
$\displaystyle=v|^{A}-R_{B}^{A}(R_{A}^{B}v|^{A}+t_{A}^{B})$
$\displaystyle=v|^{A}-R_{B}^{A}R_{A}^{B}v|^{A}-R_{B}^{A}t_{A}^{B}$
$\displaystyle=-R_{B}^{A}t_{A}^{B}$

∎

###  B-D Proof of Theorem 1

###### Proof.

Consider any point $p|^{B_{k+1}}\in\mathbb{R}^{3}$. When expressed in frame
$B_{k}$, we have

$\displaystyle\left\|p|^{B_{k}}-\hat{p}|^{B_{k}}\right\|$
$\displaystyle=\left\|\left(R_{B_{k+1}}^{B_{k}}p|^{B_{k+1}}+t_{B_{k+1}}^{B_{k}}\right)-\left(\hat{R}_{B_{k+1}}^{B_{k}}p|^{B_{k+1}}+\hat{t}_{B_{k+1}}^{B_{k}}\right)\right\|$
$\displaystyle=\left\|\left(R_{B_{k+1}}^{B_{k}}-\hat{R}_{B_{k+1}}^{B_{k}}\right)p|^{B_{k+1}}+\left(t_{B_{k+1}}^{B_{k}}-\hat{t}_{B_{k+1}}^{B_{k}}\right)\right\|$
$\displaystyle=\left\|\left(R_{B_{k+1}}^{B_{k}}-\hat{R}_{B_{k+1}}^{B_{k}}\right)p|^{B_{k+1}}+\left(-R_{B_{k+1}}^{B_{k}}t_{B_{k}}^{B_{k+1}}+\hat{R}_{B_{k+1}}^{B_{k}}\hat{t}_{B_{k}}^{B_{k+1}}\right)\right\|$

where in the last step we used the relationship
$t_{B}^{A}=-R_{B}^{A}t_{A}^{B}$ (see Lemma 6). Adding and subtracting
$R_{B_{k+1}}^{B_{k}}\hat{t}_{B_{k}}^{B_{k+1}}$ inside the norm yields

$\displaystyle\left\|p|^{B_{k}}-\hat{p}|^{B_{k}}\right\|$
$\displaystyle=\left\|\left(R_{B_{k+1}}^{B_{k}}-\hat{R}_{B_{k+1}}^{B_{k}}\right)p|^{B_{k+1}}+\left(-R_{B_{k+1}}^{B_{k}}t_{B_{k}}^{B_{k+1}}+\hat{R}_{B_{k+1}}^{B_{k}}\hat{t}_{B_{k}}^{B_{k+1}}\right)+\left(R_{B_{k+1}}^{B_{k}}\hat{t}_{B_{k}}^{B_{k+1}}-R_{B_{k+1}}^{B_{k}}\hat{t}_{B_{k}}^{B_{k+1}}\right)\right\|$
$\displaystyle=\left\|\left(R_{B_{k+1}}^{B_{k}}-\hat{R}_{B_{k+1}}^{B_{k}}\right)p|^{B_{k+1}}+R_{B_{k+1}}^{B_{k}}\left(-t_{B_{k}}^{B_{k+1}}+\hat{t}_{B_{k}}^{B_{k+1}}\right)+\left(\hat{R}_{B_{k+1}}^{B_{k}}-R_{B_{k+1}}^{B_{k}}\right)\hat{t}_{B_{k}}^{B_{k+1}}\right\|$
$\displaystyle=\left\|\left(R_{B_{k+1}}^{B_{k}}-\hat{R}_{B_{k+1}}^{B_{k}}\right)\left(p|^{B_{k+1}}-\hat{t}_{B_{k}}^{B_{k+1}}\right)+R_{B_{k+1}}^{B_{k}}\left(-t_{B_{k}}^{B_{k+1}}+\hat{t}_{B_{k}}^{B_{k+1}}\right)\right\|$

Now using the triangle inequality and simplifying,

$\displaystyle\left\|p|^{B_{k}}-\hat{p}|^{B_{k}}\right\|$
$\displaystyle\leq\left\|R_{B_{k+1}}^{B_{k}}-\hat{R}_{B_{k+1}}^{B_{k}}\right\|\left\|p|^{B_{k+1}}-\hat{t}_{B_{k}}^{B_{k+1}}\right\|+\left\|R_{B_{k+1}}^{B_{k}}\left(-t_{B_{k}}^{B_{k+1}}+\hat{t}_{B_{k}}^{B_{k+1}}\right)\right\|$
$\displaystyle=\left\|R_{B_{k+1}}^{B_{k}}-\hat{R}_{B_{k+1}}^{B_{k}}\right\|\left\|p|^{B_{k+1}}-\hat{t}_{B_{k}}^{B_{k+1}}\right\|+\left\|-t_{B_{k}}^{B_{k+1}}+\hat{t}_{B_{k}}^{B_{k+1}}\right\|$
$\displaystyle=\left\|\left(R_{B_{k+1}}^{B_{k}}-\hat{R}_{B_{k+1}}^{B_{k}}\right)^{T}\right\|\left\|p|^{B_{k+1}}-\hat{t}_{B_{k}}^{B_{k+1}}\right\|+\left\|-t_{B_{k}}^{B_{k+1}}+\hat{t}_{B_{k}}^{B_{k+1}}\right\|$
$\displaystyle=\left\|R_{B_{k}}^{B_{k+1}}-\hat{R}_{B_{k}}^{B_{k+1}}\right\|\left\|p|^{B_{k+1}}-\hat{t}_{B_{k}}^{B_{k+1}}\right\|+\left\|t_{B_{k}}^{B_{k+1}}-\hat{t}_{B_{k}}^{B_{k+1}}\right\|$
$\displaystyle\leq\epsilon_{R,k}\left\|p|^{B_{k+1}}-\hat{t}_{B_{k}}^{B_{k+1}}\right\|+\epsilon_{t,k}$

Define $\mathcal{P}\subset\mathbb{R}^{3}$ as the set of points $p|^{B_{k}}$
that can correspond to $p|^{B_{k+1}}$:

$\displaystyle\mathcal{P}=\\{p\in\mathbb{R}^{3}:\left\|p-\hat{p}|^{B_{k}}\right\|\leq\Delta\\},$

where
$\Delta=\epsilon_{R,k}\left\|p|^{B_{k+1}}-\hat{t}_{B_{k}}^{B_{k+1}}\right\|+\epsilon_{t,k}$.
Then,

$\displaystyle d(p|^{B_{k+1}})$
$\displaystyle\geq\min_{\tilde{p}\in\mathcal{P}}d(\tilde{p})$
$\displaystyle\geq d(\hat{p}|^{B_{k}})-\Delta$ $\displaystyle\geq
d_{M}^{k}(\hat{p}|^{M})-\epsilon_{R,k}\left\|p|^{B_{k+1}}-\hat{t}_{B_{k}}^{B_{k+1}}\right\|-\epsilon_{t,k}$

where
$\hat{p}|^{B_{k}}=\hat{R}_{B_{k+1}}^{B_{k}}p|^{B_{k+1}}+\hat{t}_{B_{k+1}}^{B_{k}}$,
and $\hat{p}|^{M}=\hat{R}_{B_{k}}^{M}\hat{p}|^{B_{k}}+\hat{t}_{B_{k}}^{M}$.
Therefore, we have

$\displaystyle d(p|^{B_{k+1}})$ $\displaystyle\geq
d_{M}^{k}(\hat{p}|^{M})-\epsilon_{R}\left\|p|^{B_{k+1}}-\hat{t}_{B_{k}}^{B_{k+1}}\right\|-\epsilon_{t}$
$\displaystyle=d_{M}^{k}(\hat{p}|^{M})-\epsilon_{R}\left\|\left(\hat{R}_{M}^{B_{k+1}}\hat{p}|^{M}+\hat{t}_{M}^{B_{k+1}}\right)-\hat{t}_{B_{k}}^{B_{k+1}}\right\|-\epsilon_{t}$
$\displaystyle=d_{M}^{k+1}(\hat{p}|^{M})$

by defining $d_{M}^{k+1}:\mathbb{R}^{3}\to\mathbb{R}$ as

$\displaystyle
d_{M}^{k+1}(\hat{p}|^{M})=d_{M}^{k}(\hat{p}|^{M})-\epsilon_{R}\left\|\hat{R}_{M}^{B_{k+1}}\hat{p}|^{M}+\hat{t}_{M}^{B_{k+1}}-\hat{t}_{B_{k}}^{B_{k+1}}\right\|-\epsilon_{t}.$

This completes the proof. ∎

  *[ESDF]: Euclidean Signed Distance Field
  *[C-VO]: Certified Visual Odometry
  *[VO]: Visual Odometry
  *[]:
  *[C-ESDF]: Certified   ()
  *[RGBD]: RGB-Depth
  *[IMU]: Inertial Measurement Unit
  *[FoV]: Field of View
  *[SDF]: Signed Distance Field
  *[ VO]: Visual Odometry
  *[SFC]: Safe Flight Corridors
  *[TLS]: Truncated Least Squares
  *[GNC]: Graduated-Nonconvexity
  *[WLS]: Weighted Least Squares
  *[TSDF]: Truncated Signed Distance Field
  *[SVD]: Singular Value Decomposition