\section*{Acknowledgements}
We would like to thank Adam Caccavale, Gadi Camps, and Jun En Low for their insights throughout this project.

\balance
\bibliographystyle{IEEEtran.bst}

\section{Introduction}

\begin{figure}[]
         \centering         \includegraphics[width=0.49\textwidth]{figures/title/title.png}
        \caption{(a) Ground-truth of the Stonehenge scene, (b) Poisson Point Process (PPP) of the scene represented as a point cloud, (c) Probabilistically Unsafe Robot Region (PURR) of scene, (d) Generated safe paths from our method.}
        \label{fig:three graphs}
\end{figure}

Constructing an environment model from onboard sensors, such as RGB(-D) cameras, lidar, or touch sensors, is a fundamental challenge for any autonomous system. Recently, Neural Radiance Fields (NeRFs) \cite{mildenhall2020a} have emerged as a promising 3D scene representation with potential applications in a variety of robotics domains including SLAM \cite{sucarIMAPImplicitMapping2021}, pose estimation \cite{yen2021inerf, yen2022nerfsupervision}, reinforcement learning \cite{driess2022learning}, and grasping \cite{ichnowski2021dex}. NeRFs offer several potnetial benefits over traditional scene representations: they can be trained using only monocular RGB images, they provide a continuous representation of obstacle geometry, and they represent scene geometry accurately even when specular or transparent materials are present while sensors such as depth cameras and lidar often fail \cite{yen2022nerfsupervision, ichnowski2021dex}. Using current implementations \cite{mueller2022instant, nerfstudio}, NeRFs can be trained in seconds using only RGB images captured from monocular cameras, making onboard, online NeRF training a viable option for  robotic systems.

However, NeRFs do not directly give information about spatial occupancy, which poses a challenge in using NeRF models for safe robot navigation. In other 3D scene representations, such as (watertight) triangle meshes \cite{edelsbrunner2003surface}, occupancy grids \cite{elfes1989using}, or Signed Distance Fields (SDFs) \cite{osher2003level}, occupancy is well-defined and simple to query. NeRFs, however, do not admit simple point-wise occupancy queries, since they represent the scene geometry implicitly through a continuous volumetric density field. For this reason, integrating NeRF models into robotic planners with mathematical safety guarantees remains an open problem.

In this work we develop a framework for robot trajectory planning that can generate trajectories through a NeRF scene with probabilistic safety guarantees. To do this, we propose a mathematical interpretation of a NeRF as a Poisson Point Process (PPP), which allows for the rigorous computation of collision probabilities for a robot moving through a NeRF scene. We further introduce a novel scene representation, a Probabilistically Unsafe Robot Region (PURR), that convolves the robot geometry with the NeRF to yield a 3D map of all robot positions with collision probabilities less than a user-specified threshold. Finally, we propose a fast, chance-constrained trajectory planner that uses the PURR to ensure the trajectories are collision free up to the user-specified probability threshold. Our method can compute multiple trajectory options from a cold start in about 1 sec, and can re-solve for single trajectories online at 50 Hz, while guaranteeing safety. This is 5 times faster at initial planning and 100 times faster at online re-planning than existing NeRF-based trajectory planners that provide no safety guarantee \cite{adamkiewicz2022vision}.

The key theoretical advance underpinning our results is the novel interpretation of the NeRF as a PPP.  Existing works on radiance fields either ignore the underlying probabilistic interpretation of the field or treat it as a nuisance.
A naive approach is to convert the NeRF representation into a more traditional deterministic mesh or occupancy representation.  We argue that such conversions are computationally slow, and they destroy any potential mathematical safety guarantees for a downstream planner.  For example, generating a triangle mesh (e.g., using marching cubes \cite{lorensen1987marchinga}) that represents a level set of the density field requires the arbitrary selection of a density cutoff value, and collapses the uncertainty represented by the density field into a binary occupancy measure.  In contrast, our method computes rigorous collision probabilities using the NeRF density directly.

We provide simulation studies to show that our planner generates safe, but not overly-conservative, trajectories through the environment. We contrast our paths to those generated using a level-set based environment representation and those from prior work \cite{adamkiewicz2022vision}. We find that the paths our method generates are more intuitive and easier to tune that these baselines, as collision is strictly defined through violation of a collision probability as opposed to violation of an arbitrary level set of the density. We show our method to be real-time, replanning online at 50Hz on a desktop computer, compared to the gradient descent-based planner proposed in \cite{adamkiewicz2022vision}, which requires approximately 4 sec for replanning.

The rest of this paper is organized as follows.  In Section~\ref{Sec:RelatedWork} we discuss related work.  In Section~\ref{Sec:Background} we review background concepts from  NeRFs, and in Section~\ref{Sec:PPP} we derive the Poisson Point Process interpretation of the NeRF.  In Section~\ref{Sec:CollisionProbability} we compute collision probabilities for a robot in a NeRF environment, and in Section~\ref{Sec:CollisionAvoidance} we present our trajectory planning algorithm, CATNIPS.  Section~\ref{Sec:Results} gives our simulation results and Conclusions are in Section~\ref{Sec:Conclusion}.

\section{Neural Radiance Fields (NeRFs)}
\label{Sec:Background}
In this section, we introduce the mathematical preliminaries and notation used in NeRFs. A NeRF is a neural network that stores a density and color field over the 3D environment.  When coupled with a differentiable image rendering model (usually a differentiable version of ray tracing), the NeRF can be trained from a collection of RGB images with known camera poses, and can generate photo-realistic synthetic images rendered from camera view points that are different from the training images.

More specifically, the NeRF is a pair of functions $(\rho(\mathbf{p}),c(\mathbf{p},\mathbf{d}))$.  The density function, $\rho:\mathbb{R}^3 \mapsto \mathbb{R}_{>0}$, maps a 3D location $\mathbf{p} = (x, y, z)$ to a positive density value $\rho$ that encodes the differential probability of a light ray stopping at that point.\footnote{This density field can be stored entirely as a multi-layer perceptron (MLP), as in the original NeRF work \cite{mildenhall2020a}, as a function interpolated on a discrete voxel grid \cite{plenoxels}, or using a combination of interpolated voxel features and an MLP decoder \cite{mueller2022instant,plenoctrees}.  Our method can work with any of these representations.}  The radiance (i.e., RGB color) function $c:\mathbb{R}^3 \times \mathbb{R}^2 \mapsto \mathbb{R}^3$ maps a 3D location $\mathbf{p} = (x, y, z)$ and camera view direction $\mathbf{d} = (\theta, \phi)$ to an emitted RGB color $c$ represented as a vector in $\mathbb{R}^3$. In this paper, we focuse specifically on the density function $\rho(\mathbf{p})$ as a proxy for occupancy, which should ideally be zero in free space and take on large values in occupied space.
 We use this $\rho(\mathbf{p})$ function as a map representation for planning robot trajectories.  We also define $C(\mathbf{o}, \mathbf{d}) \in [0, 1]^3$ as the rendered pixel color in an image when taking the expected color value from the NeRF along a ray $\mathbf{r}(t; \mathbf{o}, \mathbf{d})$ with camera origin $\mathbf{o}$ and pixel orientation $\mathbf{d}$, where $\mathbf{r}(t) = \mathbf{o} + t\cdot \mathbf{d}$. The rendered color is given by

\begin{equation}
\label{Eq:RayTracing}
C(\mathbf{o}, \mathbf{d})= \int_{t_n}^{t_f} \rho\left(\mathbf{r}(t)\right) \; e^{-\int_{t_n}^{t} \rho(\mathbf{r}(\tau)) d\tau} \; \mathbf{c}(\mathbf{r}(t), \mathbf{d})\; dt,
\end{equation}
where we only integrate points along the ray between $t_n$ and $t_f$ (i.e. the near and far planes).  In practice, this integral is evaluated numerically using Monte Carlo integration with stratified sampling.  The resulting rendering equation (\ref{Eq:RayTracing}) is differentiable.

A rendered image $I_i$ is then an array of pixel colors associated with a single camera pose, where the color of pixel $j$ in image $I_i$ is given by $C(\mathbf{o}_i, \mathbf{d}_{ij})$ with associated origin $\mathbf{o}_i$ (determined by the camera) and direction $\mathbf{d}_{ij}$ computed with an angular offset from the camera optical axis. We denote the set of pixel indices for image $I_i$ as $\mathcal{I}_i$.  The corresponding ground truth image $\bar{I}_i$ is an array of pixels with colors $\bar{C}_{ij}$.  A dataset D for training a NeRF consists of a collection of such ground truth images with known poses. The parameters of the NeRF are trained by minimizing the loss function
\begin{equation}
\label{Eq:PhotometricLoss}
J(\theta) = \frac{1}{|D|}\sum_{i\in D}\frac{1}{|\mathcal{I}_i|}\sum_{j\in \mathcal{I}_i}||C(\mathbf{o}_i, \mathbf{d}_{ij}; \theta) - \bar{C}_{ij}||_2^2,
\end{equation}
where $\theta$ are the parameters of the neural networks representing the density and radiance fields $\rho$ and $c$, which appear in the computation of the pixel color $C(\mathbf{o}_i, \mathbf{d}_{ij}; \theta)$ through the rendering equation (\ref{Eq:RayTracing}). This mean squared error is called the photometric error (or photomoetric loss) and is optimized with standard stochastic gradient descent tools in, e.g. Pytorch. Intuitively, the goal is to train the network so that the synthetic images generated from the NeRF match the training images at the specified camera poses as closely as possible.

While the camera poses are required to find $\mathbf{o}_i$ and $\mathbf{d}_{ij}$ for each pixel to train the NeRF, a standard pipeline has emerged that takes images without camera poses, uses a classical structure-from-motion algorithm (e.g., COLMAP \cite{schonberger2016structure}) to estimate the camera poses, and supervises the NeRF training with these poses. Recent methods also optimize the camera poses jointly with the NeRF weights to improve performance \cite{nerfstudio}. Hence, in practice a NeRF model can be obtained from only RGB images (without camera poses).

\section{NeRF Density as a Poission Point Process}
\label{Sec:PPP}
In this section, we show that a NeRF density field may be interpreted as a Poisson Point Process (PPP) and the NeRF color and density fields together represent a ``marked'' PPP \cite{Kingman:1993, chiu2013}. To do this, we demonstrate that the NeRF volumetric rendering equation is precisely the computation that is required to compute expected pixel color if the color and density are interpreted as a marked PPP.

This connection is significant since identifying the NeRF density field as a PPP enables computation of probabilistic quantities, such as the probability of a given volume being occupied (e.g., of a robot body colliding with the NeRF), or the entropy in the NeRF model. This also settles a debate in the literature about the interpretation of the NeRF density, and paves the way for practical utility in other domains beyond safety (e.g., in active sensing and active view selection). In short, we find that the NeRF density encodes a probabilistic model of the geometry of the scene, the uncertainties of which can be rigorously quantified through the PPP interpretation.

\subsection{Poisson Point Processes}

Here we review the definition and properties of the Poisson Point Process (PPP), a stochastic process that models the distribution of a random collection of points in a continuous space. Much of this discussion is drawn from \cite{Kingman:1993}, to which we refer the reader for a more detailed and rigorous treatment.

First, we recall that a discrete random variable (RV) $N$ that takes values in $\mathbb{N}$ is said to have a Poisson distribution with parameter $\lambda > 0$ if its probability mass function is given by
\begin{equation*}
Pr(N = m) = \frac{\lambda^m \exp(-\lambda)}{m!}.
\end{equation*}
Poisson RVs are often used to model the distribution of the number of discrete events in a fixed amount of time (e.g., customers arriving at a store), or over a fixed region of space (e.g. the number of rides hailed daily in a given neighborhood). The PPP naturally extends this concept to the distribution over the number of points in any subset of a multi-dimensional Euclidean space.
\begin{definition}[Poisson Point Process]
\label{def:ippp}
Consider a random process $N$ on $\mathbb{R}^n$ that maps subsets\footnote{The subsets $B$ must be Lebesgue measureable.} $B \subset \mathbb{R}^n$ of the state space to the random number $N(B)$ of points that lie in $B$. We say $N$ is a Poisson Point Process (PPP) with intensity $\lambda: \mathbb{R}^n \mapsto \mathbb{R}_+$ if:
\begin{enumerate}[(i)]
\item The number of points $N(B)$ that lie in $B$ is a Poisson RV with distribution
\begin{equation*}
Pr(N(B) = m) = \frac{\Lambda(B)^m \exp(-\Lambda(B))}{m!},
\end{equation*}
where $\Lambda(B) = \int_{x\in B} \lambda(x) dx$.
\item For $k$ disjoint subsets $B_1, \ldots B_k \subset \mathbb{R}^n$, the number of points in each subset, $N(B_1), \ldots, N(B_k)$, are independent RVs.
\end{enumerate}
\end{definition}
This is sometimes refered to as the \emph{inhomogeneous} PPP since the intensity $\lambda$ is a function of the spatial variable $x$.  If the intensity is constant over $x$, this is called the \emph{homogeneous} PPP.

The PPP encodes the randomness over both the \emph{number} and the \emph{location} of random points. An important quantity for such processes is the ``void probability,'' or the probability that a given set $B$ is empty. The void probability is given by
\begin{equation}
Pr(N(B) = 0) = \exp\left[-\int_{B}\lambda(x)dx\right]. \label{eq:void}
\end{equation}
Thus, intuitively, the void probability shrinks as either the intensity $\lambda$ increases, or the set $B$ grows larger.

Finally, we note that PPPs may be ``marked'' or ``colored'' with various quantities using a deterministic labeling function $c(x): \mathbb{R}^n \mapsto \mathcal{C}$. Using the statistics of the underlying PPP, it is straightforward to compute the statistics of the labels for the points appearing in a set; see \cite[Ch.~5]{Kingman:1993} for a detailed discussion.

\subsection{``Rendering'' a Marked PPP}\label{ppp-proof}
\begin{figure}[]
    \centering
         \includegraphics[width=.48\textwidth]{figures/explainers/ray_tracing.png}
        \caption{In the rendering process, the probability that the pixel color takes on the color of the infintesimally small occluding slice (green) is given by the probability that nothing exists in the unoccluded region (red). Then, the pixel color is the expectation of the color taken by varying the position of the occluding slice along the ray.}
    \label{fig:rendering_explainer}
\end{figure}

The intensity function of a PPP admits an important infinitesimal interpretation: $\lambda(x) dx$ is the probability that a point of the process lies within an infinitesimal volume $dx$ centered at $x$. We note this is closely related to the interpretation of the density field offered by the original NeRF authors \cite{mildenhall2020a}: $\rho(x)$ defines the infinitesimal probability of a ray terminating at a given point $x \in \mathbb{R}^3$. In this section, we make this connection explicit by showing the volumetric rendering procedure introduced in \cite{mildenhall2020a}, in fact, computes an expected color of a marked PPP along a given ray.

Our key problem is how to relate the ray tracing used in NeRF, a 1D process, to a 3D measure of occupancy. To do this, we model ``rendering''  as beam tracing \cite{heckbert1984}, which considers the occupancy of ``thick beams'' (i.e., the pixel area swept along a particular ray) rather than the occupancy of infinitesimal rays. In particular, when generating the color of a particular pixel from a marked PPP, we extend a rectangular prism along the pixel's ray (Fig. \ref{fig:rendering_explainer}), and return the color of the first point of the process encountered along the ray.

More specifically, for each pixel in the image, with associated ray $\mb{r}(\mb{o}, \mb{d})$, we consider a ``beam,'' (Fig. \ref{fig:rendering_explainer}, red) parameterized by a length $t \in (t_n, t_f),$

\begin{align}
    \mathcal{B}(t) \equiv \left\{\mb{o} + \mb{d}\tau + x \hat{\mb{n}}_x + y\hat{\mb{n}}_y \mid \lvert x \rvert \leq \frac{w}{2}, \lvert y \rvert \leq \frac{h}{2}, \tau \in [t_n, t] \right\},\label{eq:beam_def}
\end{align}
where $\hat{\mb{n}}_x, \hat{\mb{n}}_y$ are unit vectors tangent to $\mb{d}$ forming a basis in the image plane and $h, w$ are the height and width of the pixel, respectively, measured on the image plane. We say the ray terminates wherever it first encounters a point of the process, i.e., at the first depth $t$ where the set $\mathcal{B}(t)$ is nonempty.

\subsection{Equivalence of NeRF and PPP Rendering}

This brings us to our main result. Here we show that, under some assumptions about the spatial variation of the NeRF density and color, the color of a given pixel in an image rendered from a NeRF \eqref{Eq:RayTracing} is exactly the expected color of the same pixel rendered from a PPP with (scaled) intensity equal to the NeRF density $\rho$, and marking equal to the NeRF radiance $\mb{c}$.

\begin{proposition}[NeRF as PPP]
Consider a NeRF with density $\rho(\mathbf{x})$ and radiance $\mathbf{c}(\mathbf{o},\mathbf{d})$. Let $C(\mb{o}, \mb{r})$ be the color of a pixel of width $w$ and height $h$ rendered from the NeRF \eqref{Eq:RayTracing}. Assume the NeRF density and radiance are constant over lengths smaller than $R = \sqrt{\frac{w^2 + h^2}{2}}$, i.e., $\rho(\mb{x} + \mb{z}) = \rho(\mb{x}), \mb{c}(\mb{x}+\mb{z}, \mb{d}) = \mb{c}(\mb{x}, \mb{d})$ if $\lvert\lvert \mb{z} \rvert \rvert \leq R$. Then, the pixel color $C(\mb{o}, \mb{r})$ of the NeRF is the expected color of a pixel rendered from a PPP with intensity $\lambda(\mb{x}) = \frac{\rho(\mb{x})}{wh}$ and marking $\mb{c}(\mb{x}, \mb{d}).$

\end{proposition}
\begin{proof}
Let us consider a ray $\mathbf{r}(t) = \mathbf{o} + t \cdot \mathbf{d},$ where $t \in [t_n, t_f]$. We consider again the corresponding beam $\mathcal{B}(t)$ \eqref{eq:beam_def}, the rectangular prism created by sweeping the pixel area along $\mb{r}$ from $t_n$ to $t$. For this set, we can compute the intensity $\Lambda\big(\mathcal{B}(t)\big)$ by computing the integral
\begin{align*}
    \Lambda\big(\mathcal{B}(t)\big) &= \int_{\mb{x} \in \mathcal{B}(t)} \lambda(\mb{x}) d\mb{x} \\
    &= \int_{-\frac{w}{2}}^{\frac{w}{2}} \int_{-\frac{h}{2}}^{\frac{h}{2}} \int_{t_n}^{t} \frac{\rho(\mb{r}(\tau) + x \hat{\mb{n}}_x + y \hat{\mb{n}}_y)}{wh} d\tau dy dx,\\
    &= \int_{t_n}^{t} \rho(\mb{r}(\tau)) d\tau
\end{align*}
where the last equality follows from the assumption that $\rho$ is constant for $\lvert \lvert x \hat{\mb{n}}_x + y \hat{\mb{n}}_y \rvert \rvert^2 \leq \frac{w^2 + h^2}{2}$.

Let us now consider a random variable $T_\text{min}$ which defines the distance of the first (or closest) point of the Poisson process to lie along the ray (i.e. the location of the occluding slice denoted green in Fig. \ref{fig:rendering_explainer}). We can define the cumulative distribution function of this variable by noting, for some $t > t_n$, $T_\text{min} \leq t$ if and only if $N\big(\mathcal{B}(t)\big) > 0$.
Thus, the CDF of $T_\text{min}$ can be defined using the void probability of $\mathcal{B}(t)$ \eqref{eq:void},
\begin{align*}
P(T_\text{min}\leq t) \equiv F_{T_\text{min}}(t) &= 1 - P\left(N\big(\mathcal{B}(t)\big) = 0\right), \\
 &= 1 - \exp\left[-\Lambda\big(\mathcal{B}(t)\big)\right],\\
 &= 1 - \exp\left[-\int_{t_n}^t \rho(\mathbf{r}(\tau))d\tau \right].
\end{align*}
\end{proof}

Thus, we can compute the PDF of $T_\text{min}$ by differentiating $F_{T_\text{min}}$ with respect to $t$, yielding,
\begin{equation}
    f_{T_\text{min}}(t) = \frac{d}{dt} F_{T_\text{min}}(t) = \rho(\mathbf{r}(t)) \exp\left[-\int_{t_n}^t \rho(\mathbf{r}(\tau))d\tau\right]. \label{eq:depth_likelihood}
\end{equation}
This defines a probability distribution over the extent of the unoccluded region.

Finally, we note that, since we assume the radiance of the NeRF is constant over the pixel area, the color of the point returned by the PPP rendering is equal to the marking function evaluated at $\mb{r}(T_\text{min}).$ Thus, we can compute the expected color of PPP rendering by computing the expectation of $\mathbf{c}(\mathbf{r}(T_\text{min}))$, yielding
\begin{align*}
    C(\mathbf{r}) &= \mathbb{E}\left[ \mathbf{c}(\mathbf{r}(T_\text{min})) \right],\\
    &= \int_{t_n}^{t_f} \rho(\mathbf{r}(t)) \; \mathbf{c}(\mathbf{r}(t)) \; \Theta(t) dt,
\end{align*}
where $\Theta(t) = \exp\left[-\int_{t_n}^t \rho(\mathbf{r}(\tau))d\tau\right].$ This expected color matches exactly the expression given in (\ref{Eq:RayTracing}) from the original NeRF paper \cite{mildenhall2020a}, completing the proof.

We visually show the relationship between the PPP and the NeRF in Fig.~\ref{fig:ppp-st-overlay}, where we generate a point cloud randomly drawn from the PPP (blue spheres) superimposed on the NeRF rendering of the same scene.  The correspondence in geometry of the NeRF scene and the PPP point cloud is clear.

\begin{figure}[]
     \centering
        \includegraphics[width=0.49\textwidth]{figures/compare_GT_PPP.png}
        \caption{Overlay of a realization of the PPP with the ground-truth mesh of Stonehenge. The two have strong spatial agreement.}
        \label{fig:ppp-st-overlay}
\end{figure}
Note that while area-averaging of $\lambda$ to $\rho$ yielded the rendered color as a line integral as opposed to a volume integral, we have lost information about the $\lambda$ field when using a learning framework to learn $\rho$. In fact, we conjecture the observed aliasing phenomena in which NeRFs fail at different scales \cite{mipnerf} is due to simplistic area-averaging of $\lambda$ to retrieve $\rho$. The success of works like Mip-NeRF \cite{mipnerf} that reason about the pixel not as a projection of a ray, but along a frustrum and evaluating rendering as a volume integral (i.e., learning $\lambda$ rather than $\rho$) gives us reason to believe that learning the parameters of the PPP directly could yield higher quality geometry.

\subsection{Discussion of PPP Interpretation}

An important advantage of taking a PPP interpretation of the NeRF density is that it allows us to leverage well-studied properties of PPPs when reasoning about NeRFs. Specifically, uncertainty metrics such as likelihood and entropy are well-defined for PPPs. Suppose we measure a point cloud of our environment using an onboard lidar or depth camera; i.e., for a set of rays $\{\mb{r}_1, \ldots, \mb{r}_k\}$ we obtain noiseless depth measurements $\{d_1, \ldots d_k\}$. We can write the likelihood of obtaining these depths under our NeRF using \eqref{eq:depth_likelihood},

\begin{equation} \label{ppp-ll}
\log P(d_1, \ldots d_k) = \sum_{i=0}^k \log \rho(\mb{r}(d_i)) - \int_{t_n}^{d_i} \rho(\mb{r}(t))dt.
\end{equation}

If, say, the robot's state is uncertain, and the depth measurements are corrupted by noise, this likelihood can be used in the computation of Bayes' rule for pose estimation. Since previous literature on NeRFs contained no such likelihood interpretation, existing approaches to state estimation  \cite{yen2021inerf, adamkiewicz2022vision} instead only minimize a photometric loss as a proxy for maximum likelihood estimation.

Further, notions of entropy and mutual information can be generalized to point processes, as discussed in \cite{ppp-entropy}. In particular, the entropy of a point process over a set $B$ is defined as:

\begin{align}
h(B) &\equiv \int_B \lambda(\mb{x}) (1 - \log \lambda(\mb{x}))d \mb{x}\\
&= \frac{1}{wh} \int_B \rho(\mb{x}) (1 + \log wh - \log \rho(\mb{x})) d\mb{x} \label{ppp-entropy}.
\end{align}
Thus, $h$ can be used as a measure of the uncertainty of a NeRF over some set $B$, which would be useful to reason about which parts of the NeRF are well-supervised for problems such as active perception.

Finally, the PPP interpretation of the NeRF allows us to provide a novel perspective on NeRF training: minimizing the photometric loss \eqref{Eq:PhotometricLoss} proposed in \cite{mildenhall2020a} can be interpreted as performing moment-matching \cite[Ch.~4]{vaart_1998} on the first moment of the color distribution along the supervised rays. In particular, the photometric loss will be zero if the color rendered from the NeRF (i.e., an expected color along the ray of a PPP) matches the sample distribution (i.e., the color label in the dataset). We believe our probabilistic interpretation of the NeRF density could also inspire other loss functions beyond \eqref{Eq:PhotometricLoss}, to perform other methods of parameter estimation such as maximum likelihood estimation, expectation maximization, and so on.

\section{Computing Collision Probability with NeRF Scenes}
\label{Sec:CollisionProbability}
In this section, we leverage the probabilistic interpretation of NeRFs to evaluate the probability of collision between a robot body and the NeRF, a NeRF and a NeRF, and provide extensions to state uncertainty in Appendix \ref{app:state_uncertain_purr}.

\subsection{Collision Probability with a Robot Body}\label{coll_prob}
Define $B(\mathbf{p},R) \subset \mathbb{R}^n$ as the robot body parameterized by its pose $\mathcal{T} = (\mathbf{p}, R)$, (i.e., the set of points occupied by the robot with position $\textbf{p}\in\mathbb{R}^3$ and orientation $R\in \mathbb{SO}(3)$) and consider an environment represented as a NeRF with density field $\rho(\mathbf{p})$, which we have shown to be equivalent to the PPP field through $\lambda(\mathbf{p}) = \frac{\rho(\mathbf{p})}{wh}$.
The collision probability, or the probability that a point from a PPP intersects with the robot body, is equivalent to the probability that $B$ is non-empty. This is the complement of the void probability (\ref{eq:void}) and is trivially extended to NeRF environments through the conversion from $\lambda$ to $\rho$
\begin{equation}\label{chance_constraint}
    Pr(N({B}) > 0) = 1 - \exp\left[-\int_{{B}} \frac{\rho(x)}{wh} \; dx \right],
\end{equation}
which naturally leads to our probabilisitc notion of safety.

\begin{definition}[Probabilisitically Safe]
A robot body parametrized by its pose $B(\mathbf{p}, R)$ is probabilistically safe if the collision probability given in (\ref{chance_constraint})
 satisfies $Pr(N(B(\mathbf{p}, R))) \leq \sigma$, for some desired probability threshold $\sigma$.
\end{definition}

\subsection{Collision Probability between Two NeRF Objects}
Using the PPP formulation, we can extend collision not only between a NeRF and an arbitrary set, but also between two or more NeRF objects. For simplicity, we derive collision for two NeRF objects, which we define by their density fields $\rho_1$ and $\rho_2$. These fields are now parametrized by the pose of the objects $\rho_i(x ; \mathbf{p}_i, R_i)$ due to rigid body motion. A point $x \in \mathbb{R}^3$ is in collision with both objects if $\rho_1(x) > 0$ and $\rho_2(x) > 0$. We then define the set
\begin{equation}
    K(\mathbf{p}_1, R_1, \mathbf{p}_2, R_2) \equiv \left\{x | \rho_1(x) > 0 \; \text{and} \; \rho_2(x) > 0 \right\}
\end{equation}

as the maximal collision set. In other words, this is the largest possible set where collisions can exclusively happen.

We restrict our analysis to be within $K$, as points outside K do not contribute to collision. We can then use the same argument as the body-to-NeRF scenario to derive collision. Because they are different objects, the density fields $\rho_1$ and $\rho_2$ are spatially independent. Hence the probability that
$K$ is non-empty is the product of the probability of $K$ being non-empty due to $\rho_1$ and the probability of $K$ being non-empty due to $\rho_2$. This allows us to directly use (\ref{chance_constraint}) to calculate the NeRF-NeRF collision probability

\begin{equation}
\begin{split}\label{multi-object}
        Pr(N(K) > 0) = &\left(1 - \exp\left[-\int_{K} \frac{\rho_1({x})}{wh} \; dx \right]\right) \times \\
    &\left(1 - \exp\left[-\int_{K} \frac{\rho_2({x})}{wh} \; dx \right]\right).
\end{split}
\end{equation}
The scaling factor(s) can be adjusted to reflect the camera parameters used to train a particular object if different cameras were used to train the objects.

The maximal collision set K need not be explicitly found when evaluating (\ref{multi-object}), as multiplying the integrands with an indicator function
\begin{equation}
\phi(x) = \begin{cases}
      1 & \rho_1(x)\times \rho_2(x) > 0 \\
      0 & \text{otherwise}
   \end{cases}
\end{equation}

and integrating over a set containing both objects (e.g. $\mathbb{R}^3$) yields identical results. While we do not use this computation in the remainder of this work, this could be used to quantify collision probabilities between multiple NeRF objects in tasks such as manipulating sensed objects (represented as a NeRF) through a sensed environment (also as a NeRF).

\section{Chance-Constrained Trajectory Generation in NeRFs}
\label{Sec:CollisionAvoidance}

We now consider the problem of real-time trajectory planning for a robot in an environment represented as a NeRF, subject to constraints on the probability of collision \eqref{chance_constraint}. Our proposed algorithm, CATNIPS, has two parts: we first generate a lightweight, voxel-based scene representation, which we term a  Probabilistically Unsafe Robot Region (PURR), that encodes the robot locations that satisfy the collision constraint for a particular robot geometry. We then use Bézier curves to plan safe trajectories for a robot traversing the PURR subject to the probabilistic collision constraint. By assuming differential flatness of our robot, we can guarantee dynamic feasibility of our solution when planning in the flat output space. Our solution is real-time and we show that it is not overly-conservative.

\subsection{Generating the PURR}
The PURR is a binary, voxelized representation of the NeRF that indicates collision in $\mathbb{R}^3$. If the robot's position is located within a free voxel in the PURR, the robot is probabilistically safe, i.e., the chance collision constraint is guaranteed to be satisfied.  Otherwise, safety is not guaranteed---the robot may be in violation of the chance constraint.

Similar to classical configuration space planning \cite{lozano1990spatial}, the core idea behind the PURR is to inflate the occupied space in the map such that planning a path through free space in the inflated map corresponds to a robot trajectory that satisfies the collision probability constraint in the underlying NeRF map.  We essentially ``inflate'' the NeRF density function to account for both the robot geometry and the chance constraint.

Fig.~\ref{fig:purr_explainer} shows a schematic of the PURR generation process. We first voxelize the space of the map and label each cell with the integral of the NeRF density (normalized by the pixel dimensions) over each cell to give the voxelized \emph{cell} intensity grid, $\mathbb{I}_c$.  This computation yields a log-probability of occupancy for each cell.  We then take the Minkowski sum between a sphere bounding the robot and one underlying voxel cell to produce the set of all voxels that the robot could be touching, in any orientation, if it were located in each cell;  we call this the robot kernel, $\mathbb{K}$. Finally, we convolve the robot kernel $\mathbb{K}$ with the cell intensity grid $\mathbb{I}_c$ and exponentiate the result to obtain the \emph{robot} intensity grid, $\mathbb{I}_r$.  The \emph{robot} intensity grid gives, in each cell, an upper bound on the probability that a robot, whose position is somewhere in that cell, is in collision with the NeRF.
Finally, we threshold the robot intensity grid with the user defined collision probability threshold $\sigma$ to get the binary PURR map.  These operations are described in more mathematical detail as follows.

\begin{figure}[t]
         \centering
         \includegraphics[width=.49\textwidth]{figures/explainers/purr_generate.png}
         \caption{NeRF to PURR pipeline. (1) A density grid is sampled from the NeRF, which is then trilinearly interpolated and integrated over a particular voxel to retrieve the cell intensity grid. (2) A robot kernel is generated by taking the Minkowski sum between the minimal bounding sphere of the robot and a single voxel. (3) The kernel is used in a \texttt{Conv3D} operation with the cell intensity voxel grid to create the robot intensity grid, which we then threshold to create the PURR.}
         \label{fig:purr_explainer}
\end{figure}

\subsubsection{Cell Intensity Grid}
The \emph{cell} intensity grid $\mathcal{I}_c$ computes, for each grid cell, the negative log-probability of no collisions occuring in voxel $v_{ijk}$
\begin{equation}
\label{Eq:CellInt}
\mathbb{I}_c(v_{ijk}) = \int_{v_{ijk}} \frac{\rho(x)}{wh} dx.
\end{equation} This integral, in general, cannot be computed analytically since the density $\rho$ is typically represented using a neural network.  We compute a high-quality approximation of this integral using a trilinear interpolation scheme.  In fact, if the NeRF density uses an underlying voxel-based representation (as do the most high-speed and high quality NeRF variants in the literature \cite{mueller2022instant,plenoxels}) our integral of the trilinear interpolation is exact.

We first discretize the environment spatially, using a rectangular grid and query the NeRF for the density values at the grid vertices. We then represent the continuous density field using trilinear interpolation as \cite{max-optical},
\begin{equation}\label{trilinear}
\begin{split}
\hat{\rho}(x, y, z) = &  c_1 + c_2 x + c_3 y + c_4 z + \\
& c_5 xy + c_6 yz + c_7 xz + c_8 xyz.
\end{split}
\end{equation}
The coefficients $c_{1:8}$ are the solution of a linear system $\mathbf{A}_{1:8} c_{1:8} = \rho_{1:8}$, where $\mathbf{A}_{1:8}$ is the matrix of stacked row vectors of the terms involving cell vertex locations, and  $\rho_{1:8}$ the densities at the vertices. Note that at the vertices of the cell, $\hat{\rho}(x, y, z) = \rho(x, y, z)$. Different interpolations exist for other finite element geometries, although we only consider rectilinear cells in this work. The cell values of the \emph{cell} intensity grid are computed from a closed form analytic solution to the  integral (\ref{Eq:CellInt}) plugging in (\ref{trilinear}) for $\rho$ over the extent of the cell. The analytic expression is given in Appendix \ref{app:trilinear_analytic}.

\subsubsection{Robot Kernel}
The robot kernel $\mathbb{K}(v_{ijk})$ is a mask that indicates the neighborhood of cells around voxel $v_{ijk}$ that are considered in the computation of the collision probability when the robot position $\mathbf{p}$ is anywhere in $v_{ijk}$. We first find the Minkowski sum of the minimum bounding sphere\footnote{Inflating the robot body to a sphere removes the effects of robot orientation on safety. As a result, the PURR can be efficiently created in position space, while downstream tasks have the ability control the robot orientation without impacting safety.}
of the robot with the cell in which the robot center is located.  We then compute the smallest collection of voxels that contains this Minkowski sum.  This collection of voxels is the robot kernel, which can be efficiently convolved with the 3D voxelized grid using standard PyTorch functions.

\subsubsection{Robot Intensity Grid}
Once the robot kernel is defined, computing the collision probability for the robot in a particular cell simply requires a convolution between the kernel $\mathbb{K}$ and the cell intensity grid $\mathbb{I}_c$. In particular, we generate a \emph{robot} intensity grid $\mathbb{I}_r$, by convolving the kernel with each cell, and exponentiating the result to give the desired probability of collision as follows,
\begin{equation}
\begin{split}
\label{robo_intensity_grid}
    \mathbb{I}_r (v_{ijk}) = & 1 - \exp\left[-\sum_{v_{lmn} \in \mathbb{K}(v_{ijk})} \mathbb{I}_c (v_{lmn})\right] \\
     = & 1 - \exp\left[-\sum_{v_{lmn} \in \mathbb{K}(v_{ijk})} \int_{v_{lmn}} \frac{\hat{\rho}(x)}{wh} dx \right] \\
     \geq & 1 - \exp\left[-\int_{B(\mathcal{T})} \frac{\hat{\rho}(x)}{wh} dx \right] \forall R \in \mathbb{SO}(3), \mathbf{p} \in v_{ijk}\\
     \approx & 1 - \exp\left[-\int_{B(\mathcal{T})} \frac{\rho(x)}{wh} dx \right]\forall R \in \mathbb{SO}(3), \mathbf{p} \in v_{ijk}.
\end{split}
\end{equation}

Finally, the PURR $\mathbb{P}$ is generated by thresholding the \emph{robot} intensity grid by the collision probability threshold $\sigma$,
\begin{equation}
\label{purr_eqn}
    \mathbb{P}(v_{ijk}) = \mathbbm{1}\left[\mathbb{I}_r(v_{ijk})>\sigma\right].
\end{equation}
The resulting PURR is visualized for different probability thresholds $\sigma$ in Fig.~\ref{fig: grid-viz} for the Stonehenge NeRF environment.  As shown in the bottom row of Fig.~\ref{fig: grid-viz}, the PURR gives a similar effect as computing a voxel map based on thresholding the NeRF density directly, but the threshold for the PURR is calibrated to a precise probability of collision, and gives more regular voxelized maps, while thresholding on the density directly is not interpretable in terms of safety, and gives more irregular voxelized maps.

\begin{figure}[b]
         \centering         \includegraphics[width=0.49\textwidth]{figures/zones/zones.png}
         \caption{(Top Row) PURR with varying $\sigma$ values. (Bottom Row) density-thresholded voxel maps with varying $\rho$ values. The PURR is precisely calibrated to give a desired probability of collision, while thresholding the NeRF density directly offers no particular safety guarantee.}
         \label{fig: grid-viz}
\end{figure}

Finally, we note that the trilinear interpolation of the density (\ref{trilinear}) to compute the integral in (\ref{Eq:CellInt}) introduces a potential source of approximation error.  In practice, this error is much smaller than the over-approximation built into the various voxelization steps, yielding a PURR with a conservative probability of collision.  However, one can remove any doubt about the conservatism of the approach by introducing an upper bound on the trilinear approximation error into the formulation.  We call  this approximation error bound the \textit{collision offset factor}, $\alpha$.

\begin{definition}[Collision Offset Factor]
    The collision offset factor $\alpha$ is a map-wide upper bound on the difference between (i) the maximum collision probability achieved by integrating the NeRF density $\rho$ over the robot kernel (i.e. the ``true'' collision probability) and (ii) the collision probability using the trilinearly-interpolated density $\hat{\rho}$ from  (\ref{trilinear}),
    \begin{equation*}
    \label{Eq:CollisionOffsetFactor}
\begin{split}
    \alpha \ge \max_{i,j,k}\Big\{ &\exp\left[-\mathbb{I}_r (v_{ijk})\right] \\
    & - \min_{\mathbf{p} \in v_{ijk}, R \in \mathbb{SO}(3)}\exp \left[-\int_{B(\mathcal{T})} \lambda(x) dx\right]\Big\}.
\end{split}
\end{equation*}
\end{definition}
Finally, if $\alpha$ exists, then (\ref{purr_eqn}) and consequently our PURR are inflated with this collision offset factor to give the PURR a rigorous collision probability guarantee
\begin{equation}
\label{purr_general_eqn}
    \mathbb{P}(v_{ijk}) = \mathbbm{1}\left[1 - \exp[-\mathbb{I}_r(v_{ijk})] > \sigma - \alpha\right].
\end{equation}

\begin{theorem}
Given a collision offset $\alpha$ and the desired collision probability threshold $\sigma$, a robot with position $\mathbf{p}$ in the complement of $\mathbb{P}$ ia guaranteed to be probabilistically safe.
\end{theorem}

\begin{proof}
Following the expressions in (\ref{robo_intensity_grid}), the `$\approx$' in the last line becomes `$\ge$' for a collision offset factor, $\alpha$, that satisfies Definition \ref{Eq:CollisionOffsetFactor}.  Therefore, the resulting PURR is an upper bound on the probabilistic collision constraint of (\ref{chance_constraint}).
\end{proof}

\begin{remark}
If the discretizations match between the PURR and a voxel-based NeRF architecture, as in \cite{plenoxels} then $\alpha$ is identically 0. In practice, we still set $\alpha$ to 0 regardless of the NeRF architecture, and we find that the PURR free space is safe.
\end{remark}

\begin{remark} Although voxel representations are undesirable in memory compared to compact neural networks, we note that the PURR is binary. The memory footprint can be further reduced by using octrees and by storing the PURR with a compression scheme, e.g, with hashing.
\end{remark}

\begin{remark}
The creation of a PURR with state uncertainty $\hat{\mathbb{P}}$ is located in Appendix \ref{app:state_uncertain_purr}. The state uncertain PURR $\bar{\mathbb{P}}$ retains the original probabilistic collision guarantee.
\end{remark}

\subsection{Path Planning in the PURR}
We now turn to the problem of chance-constrained trajectory optimization through the PURR. In particular, we seek to plan a continuous path through the environment such that all points along the trajectory satisfy a chance constraint on collision (rather than enforcing the chance constraints at discrete ``knot points'' along the trajectory). In particular, we seek to find trajectories that are \emph{probabilistically safe}.

\begin{definition}[Probabilistically Safe Trajectory]
A trajectory is probabilistically safe if all points in the trajectory are point-wise probabilistically safe.
\end{definition}

We propose an algorithm containing three components used to create these safe, continuous trajectories. The first step is to find an initial, discrete path through the complement of the PURR by solving a constrained shortest path problem (Fig. \ref{fig:path_explainer}a) from our initial position $\mb{p}_0$ to our final position $\mb{p}_f$ (e.g., using A$^*$). While dynamically infeasible, this path provides a connected, collision-free path through the PURR that we will refine into a smooth, feasible trajectory. The second step is to create a ``tube'' of bounding boxes around this initial path that is not in collision with the PURR (Fig. \ref{fig:path_explainer}b). Finally, we  generate a smooth curve connecting our initial and final positions by solving a constrained convex optimization, requiring the curve to lie in the free ``tube'' generated previously (Fig. \ref{fig:path_explainer}c). We call the resulting trajectory planning algorithm Collision Avoidance Through Neural Implicit Probabilisitc Scenes (CATNIPS).  CATNIPS executes in real-time given a pre-computed PURR map.

\subsubsection{A$^*$ Search}
We first find a rough, discrete initial path through the NeRF using an A$^*$ search over the voxel grid defining the PURR complement. Specifically, given an initial position $\mb{p}_0$ and final position $\mb{p}_f$ of the robot, we find a minimum-length (measured in the Manhattan distance) path between the corresponding voxels. We search a 6-connected graph, i.e., the robot can move into neighboring free voxels of the PURR along the $x$-, $y$-, and $z$-axes. Using A$^*$ with the usual heuristic (distance to goal, not considering collision) yields a connected, collision-free, but dynamically infeasible path from the start to the goal. To accelerate computation, we use an efficient A$^*$ implementation \cite{dijkstra3d} and pre-compute the connectivity graph of the PURR to be reused for path queries online.

\subsubsection{Bounding Box Generation}

We now seek to refine the discrete, collision-free path returned by A$^*$ into a continuous trajectory that is energy-efficient, and dynamically feasible, for our robot. To this end, we will first generate a ``tube'' around our A$^*$ path that is both large (so we minimally constrain our trajectory optimization) and lies in the complement of the PURR (so trajectories in this tube still remain collision-free). We represent this tube as a union of bounding boxes, as shown in Fig.~\ref{fig:path_explainer}b.

To generate these bounding boxes, we first split the A$^*$ trajectory into straight-line segments (i.e., if the path returned by A$^*$ begins by moving along the $z$-axis for 6 voxels, we join these into a single line segment between the start and endpoint of this sequence). We then expand a bounding box around each line segment by ``marching'' each face along its normal direction until it is marginally in collision with the PURR (i.e., at least one cell on the face borders an unsafe cell). To speed collision-checking for the prospective boxes, we convert the PURR to a KD-tree representation for this step.

Once this process is complete, we now have one bounding box for each line segment in our original A$^*$ path; the union of these bounding boxes both lies in the free space of the PURR, and contains at least one connected, collision-free path between our initial and final positions. However, for voxel grids with fine spatial resolution, the number of bounding boxes will grow, adding to computational complexity; thus, as a final step we eliminate all ``similar'' bounding boxes (i.e., any bounding box whose volume has sufficient overlap with a bounding box earlier in the sequence) to generate a simplified representation.

\subsubsection{Smooth Trajectory Generation via Bézier Curves}
The final step of our planner is to generate a smooth trajectory that lies entirely in the union of the bounding boxes. To do this, we represent our trajectory as a connected series of Bézier curves. In particular, a Bézier curve in $\mathbb{R}^n$, of order $N$, is given by
\begin{equation}
    \mathbf{p}(t; \mathbf{s}_k) = \sum_{k=0}^N \binom{N}{k} (1-t)^{N-k} t^k \mathbf{s}_k,
\end{equation}
where $\mb{s}_k \in \mathbb{R}^n$ are a set of ``control points'' defining the geometry of the curve, and the curve is traced by a free parameter $t \in [0, 1]$. For any $t \in [0, 1]$, the Bézier curve $\mb{p}(t)$ is simply an interpolation of the control points $\mb{s}_k$, which means the parametric curve lies in the convex hull of the control points \cite{joybernstein}. Thus, to generate a probabilistically safe trajectory through the PURR, we find a set of Bézier curves connecting our initial position $\mb{p}_0$ and final position $\mb{p}_f$, whose control points lie in the bounding boxes generated previously; since each Bézier curve must lie in the convex hull of its control points, the entire curve will lie in the complement of the PURR. We note that this a common method for enforcing safety constraints in the path planning literature \cite{gasparetto2007new}.

\begin{figure}[t]
 \centering
 \includegraphics[width=0.49\textwidth]{figures/planner/planner_explainer.png}
 \caption{(a) Discrete path returned by A$^*$, (b) Union of bounding boxes containing subsets of the A$^*$ path whilst remaining strictly in the complement of the PURR, (c) Smooth path represented as the union of Bézier curves whose control points lie within a particular bounding box.}
 \label{fig:path_explainer}
\end{figure}

To find this trajectory, suppose we have $L$ bounding boxes $\{B_1, \ldots, B_L\}$ generated from the previous step. We then find a set of $L$ Bézier curves with control points given by $\mb{s}_k^i$, constraining all control points of the $i^\text{th}$ curve to lie in the corresponding bounding box $B_i.$ Since the Bézier curves are linear functions of the control points, we can in turn represent the $i^\text{th}$ curve as $\mathbf{p}_i(t) \equiv \beta(t) \mathbf{s}^i$, where $\mathbf{s}^i$ a concatenated vector of all the control points for curve $i$, and $\beta(t): [0, 1] \mapsto \mathbb{R}^{3 \times 3(N+1)}$ is a coefficient matrix that only depends on the curve parameter $t$. We can similarly represent higher derivatives of the curve as $\mathbf{p}_i^{(d)}(t) = \beta^{(d)}(t)\mathbf{s}^i$. We refer the reader to \cite{joybernstein} for a more detailed treatment of Bézier curves and splines.

To help smooth the spline and discourage looping behavior, we introduce the objective
\begin{equation*}
    J(\mathbf{s}^1, \ldots \mb{s}^L) = \sum_{i=1}^L \left(\int_0^1 \lvert\lvert \beta^{(d)}(t)\mathbf{s}^i\rvert\rvert_2^2 dt + \sum_{k=0}^{N-1} \lvert\lvert\mathbf{s}^i_k -\mathbf{s}^i_{k+1}\rvert\rvert_2^2
    \right),
\end{equation*}
which is quadratic in our decision variables $\mb{s}^i$. A typical choice is to penalize the snap of the trajectory ($d = 4$) as a proxy for control effort \cite{mellinger_minsnap}.

To generate our desired trajectory, we then solve the following optimization:
\begin{align}
\min_{\mb{s}^1, \ldots, \mb{s}^L} \quad& J(\mb{s}^1, \ldots,  \mb{s}^L) \label{eq:traj_qp}\\
\text{s.t.} \quad& s^i_j \in B^i, \quad &\forall i \leq L, \; j \leq N \nonumber \\
&\beta^{(d)}(1) \mb{s}^i = \beta^{(d)}(0) \mb{s}^{i+1}, &\forall i \leq L, \; d \leq D \nonumber\\
&\beta(0)\mb{s}^1 = \mb{p}_0, \nonumber\\
&\beta(1) \mb{s}^L = \mb{p}_f \nonumber
\end{align}
In particular, we constrain the control points of every segment so that $\mb{s}^i$ must lie in the corresponding bounding box $B_i$, which defines a set of linear inequaities in $\mb{s}^i$. We also enforce continuity of each spline up to a desired derivative $D$, which defines a set of linear equality constraints. Finally, we enforce the boundary conditions of our trajectory, i.e., that the curve begins at our initial position $\mb{p}_0$ and ends at our final position $\mb{p}_f$. We choose to optimize Bézier curves of order $N=5$, to balance the expressiveness of our model (which needs non-trivial derivatives up to order $d=4$) with the number of parameters needed to specify the curve.

Since our objective is quadratic in the control points, and our constraints are defined by linear inequalities and equality constraints, the resulting optimization \eqref{eq:traj_qp} is a quadratic program (QP) that can be solved in real time.

\begin{corollary}
The trajectory given by the solution of the QP \eqref{eq:traj_qp} is probabilistically safe.
\end{corollary}

\begin{proof}
    The QP \eqref{eq:traj_qp} constrains each Bézier curve to live within a bounding box that is probabilistically safe, rendering each curve safe. The resulting trajectory, given by  the union of the Bézier curves is therefore probabilistically safe.
\end{proof}

\begin{remark}
    If the robot system dynamics is differentially flat such that its position is a subset of the flat outputs, then the paths generated by the proposed QP \eqref{eq:traj_qp} (with D set to the highest derivative of position in the flat outputs) are dynamically feasible. Therefore a robot tracking a trajectory from this planner remains probabilistically safe.
\end{remark}

\begin{remark}
    We note that each segment of the trajectory returned by our planner is parameterized by a simple curve parameter $t$, which need not correspond to time. However, since we assume our system is differentiably flat, there exists a time scaling such that the curve is dynamically feasible. To resolve this, we use a simple time rescaling (as in \cite{mellinger_minsnap}) to generate the final trajectory as a function of time.

\end{remark}
\section{Numerical Results}
\label{Sec:Results}
In this section, we study our proposed chance-constrained trajectory optimizer on the simulated Stonehenge scene. Using our proposed trajectory optimizer, we generate trajectories for a quadrotor flying through the scene, and study the safety and efficiency of trajectories generated across a large number of initial and final conditions. Using the same path planning algorithm, we perform a comparison between the PURR and a baseline voxel occupancy representation obtained by thresholding the raw NeRF density at a desired density level. We also compare these methods to the authors' previous work NeRF-Nav \cite{adamkiewicz2022vision}. We demonstrate that both voxel methods are more computationally efficient than NeRF-Nav, and also generate trajectories that are safer (with fewer collisions) and more efficient (requiring less control effort). Further, we find that our method, CATNIPS, allows the user to set a clearly defined probability threshold for collision. In contrast, the density threshold baseline does not give such a probabilistic guarantee. In other words, similar behavior can be obtained from a density thresholded map, but this requires a user to tune the density threshold through trial and error to reach a desired qualitative level of safety. Even after tuning to get good empirical behavior, the baseline offers no accompanying safety guarantee.

\subsection{Algorithm Performance}
\begin{figure}[]
     \centering     \includegraphics[width=0.49\textwidth]{figures/results/paths.png}
    \caption{Generated safe paths across 100 configurations within Stonehenge.}
    \label{fig:paths-qualitative}
\end{figure}

\begin{figure}[]
 \centering
     \includegraphics[width=0.49\textwidth]{figures/results/stats_collision.png}
    \caption{Ablation of collision probability satisfaction (with max/min spread) for grid sizes of $(100)^3$, $(200)^3$, and $(300)^3$ for the same environment. Results are shown on a log10-log10 plot. The dashed-line has a slope of one, representing perfect agreement between the threshold and the empircal collision probabilities. Below the line is safe, above is in violation of the probability bound. Satisfaction of the collision probability is not very sensitive to the grid discretization. All lines follow the expected trend. }
    \label{fig: resolution_ablation}
\end{figure}

Qualitative results of the proposed method for 100 randomized start and goal locations within Stonehenge are shown in Figure \ref{fig:paths-qualitative}. The PURR was generated using a resolution of 200 voxels per side and $\sigma = 0.01$. Bézier curves of degree 8 were used in planning through the PURR free space. Moreover, because our quadrotor system is differentially flat with flat outputs in position, the robot can follow these paths with a standard differential flatness based control pipeline \cite{mellinger_minsnap}.

All paths are visually collision-free, smooth, and non-degenerate. Figure \ref{fig: grid-viz} shows the resulting PURR of Stonehenge at various values of $\sigma$, and the baseline occupancy map computed for various of the NeRF density threshold, $\rho$. We find that the PURRs are well-behaved across a wide range of $\sigma$ values, in contrast to the density threshold map. Visually, the density-based map is typically more conservative at low $\rho$ (e.g. no gap exists in the front pillars for $\rho=10^3$, but exists in $\sigma = 10^{-3}$) yet degenerates very quickly with large $\rho$. For example, the floor begins to disappear at $\rho = 10^4$ and is largely gone at $\rho=10^5$ compared to the PURR at $\sigma = 0.25$. Therefore, we argue that the well-behavedness of the PURR contributes to the safety and non-conservatism of the generated paths.

First, we validate the safety of the same 100 randomized PURR paths in Fig. \ref{fig: resolution_ablation} over various voxel grid resolutions. The figure has the empirical collision rate on the horizontal axis and the desired collision probability on the vertical, so curves below the dotted line meet the collision probability bound, while curves above violate the bound.  We see that all curves for CATNIPS are below the dotted line across all scene discretizations, indicating that the planned trajectories are probabilistically safe regardless of descritization level. We also see a trend toward less conservatism as the collision probability bound, $\sigma$, is increased. In fact, some trajectories reach just under the dotted line, illustrating that our paths are not overly conservative. Moreover, we note that the collision probability does not vary significantly based on the discretization resolution; thus, we can use rather coarse and more computationally efficient PURRs while still achieving safety.

Finally, we compare our method to two baselines: the ``baseline grid'' which computes occupancy from a sensity threshold, and NeRF-Nav \cite{adamkiewicz2022vision}, a gradient-based trajectory planner for NeRFs in Fig. \ref{fig:comparison}. We analyze their performance on two metrics: the minimum distance of the path to the ground-truth mesh (negative is within the mesh) to quantify safety, and the difference in lengths between the generated path and the shortest straight line path to quantify efficiency. We evaluate the algorithms on the same 100 randomized configurations used to evaluate the collision probability in Figure \ref{fig: resolution_ablation}. Again, we note that NeRF-Nav and the baseline paths have no safety guarantees and require parameter tuning to get a desired safety performance. Meanwhile, our method works out of the box by simply choosing the collision probability bound, $\sigma$.

We can see that NeRF-Nav trajectories are unsafe when compared to paths generated from either voxel method (negative is in collision with the mesh). As we increase the weighting on the collision penalty, we do see that the algorithm can perform safely on average. However, such a high collision penalty ($10^9$) will typically cause numerical issues in the trajectory optimizer. The trajectories also deviate from the shortest path the farthest, illustrating conservatism and non-smooth paths.

We see that our method, CATNIPS, is safer in the worst-case trajectories (higher bottom whisker) for low $\sigma = 10^{-3}$ compared to the baseline paths with low $\rho = 10^3$, and is safer on average at high values ($\sigma = 0.25, \rho = 10^5$).
Even though the baseline voxel grid can visually approximate the ground-truth (Fig. \ref{fig: grid-viz}, $\rho = 10^3$), we see that the generated trajectories can still be unsafe, rendering the baseline unreliable. In particular, for $\sigma = 0.001$, we can see that our proposed method is safer than the baseline method (with smaller interpenetration in both the mean and worst-case) with $\rho$ thresholds that generate paths of similar lengths.

We can also see the irregularity of the baseline grid come into play when analyzing the most conservative paths (i.e. paths corresponding to top whisker in the path length metric). For paths with similar safety performance between the PURR and baseline grid, baseline paths can be more conservative.

\begin{figure}[]
 \centering
     \begin{subfigure}[b]{0.49\textwidth}
         \centering
         \includegraphics[width=\textwidth]{figures/results/stats.png}
         \label{fig:statistical}
     \end{subfigure}
        \begin{subfigure}[b]{0.49\textwidth}
         \centering
         \includegraphics[width=\textwidth]{figures/results/path_max.png}
         \label{fig:path_length_max}
     \end{subfigure}
    \caption{Statistics over 100 runs at each of three parameter choices for our PPP method, the baseline grid, and NeRF-Nav \cite{adamkiewicz2022vision} in the Stonehenge environment.  Whiskers indicate max/min. The minimum distance in the top plot is to the ground-truth mesh (not known to the planner). Top: our PPP method gives an interpretable probability parameter (we can tune the lower whisker) without being overly conservative (lower path length).  The parameters of the baseline and the NeRF-Nav algorithms do not have a probability interpretation, and can lead to large collision violations (low whiskers) and overly-conservative paths. Mid and Bottom: NeRF-Nav gives longer paths, while in the worst case, the baseline is more conservative than our method for similar safety performance.}
    \label{fig:comparison}
\end{figure}

\subsection{Computation Times}
The implementation of the above algorithms are performed in Pytorch on an RTX 3090Ti and Intel i9 10900. Our method is built on top of the Pytorch Instant-NGP implementation \cite{torch-ngp}, which we did not alter. Almost no code optimizations were performed to achieve real-time performance. Under an online NeRF training scenario, the entire pipeline consisting of PURR creation (0.1 s), A* (0.1 s), and Bézier curve generation (0.02 s) operates at 4 Hz (Table \ref{comp. times}). If we assume a static NeRF scenario, the PURR can be created offline and the A* paths can be cached. More specifically, the A* implementation \cite{dijkstra3d} can simultaneously solve A* from the goal cell to all the other cells. Online replanning then consists of a lookup of the A* path from the current position ($5\times 10^{-4}$ s)  and solving for Bézier curves. Additional optimizations can be done in reducing PURR creation time, as well as investigating methods of creating a trajectory initialization quicker by processing on GPU.

Note that the proposed method produces smooth trajectories from the current position to the goal. It is important to note that in an online replanning scenario, usually only the next waypoint is tracked before the entire trajectory is updated. Thus, certain parts of our method can be adapted to only consider the vicinity of the robot, trading computation time for suboptimality. In comparison, NeRF-Nav takes much longer to converge (if at all) to a reasonable tolerance, without any safety guarantees. We believe this is primarily due to difficulty of optimizing its highly nonconvex objective and the many queries required to the density neural network.

\begin{table}[!h]
\begin{center}
\begin{threeparttable}
\begin{tabular}{ |p{2.5cm}|p{1.5cm}|p{1.5cm}|}
\hline
\multicolumn{3}{|c|}{Computation Time} \\
\hline
Operation & \textcolor{blue}{PURR} & \textcolor{orange}{NeRF-Nav}\\
\hline
Offline & 0.95 & 4.92\\
\hline
\hspace{3mm} Voxel Grid & 0.1 & 0.1\\
\hline
\hspace{3mm} A* Graph & 0.85 & 0.85\\
\hline
\hspace{3mm} Gradients & N/A & 3.97${}^{\star}$\\
\hline
Online & 0.02 & 2.0\\
\hline
\hspace{3mm} Querying Path & 5e-4 & N/A\\
\hline
\hspace{3mm} B-Spline    & 0.015 & N/A\\
\hline
\hspace{3mm} Gradients    & N/A & 2.0${}^{\star \star}$\\
\hline
\end{tabular}
    \begin{tablenotes}
      \small
      \item  ${}^\star$ 200 gradient steps.
      \item ${}^{\star \star}$ 100 gradient steps.
    \end{tablenotes}
  \end{threeparttable}
\caption{\label{comp. times} Timing results between our voxel methods (PURR and level-set) and NeRF-Nav. Because both voxel methods use similar operations, they have identical times.}
\end{center}
\end{table}

\section{Conclusion}
\label{Sec:Conclusion}
In this paper, we present a novel method for chance-constrained trajectory optimization through NeRF scenes. We present an interpretation of the NeRF density channel as a Poisson Point Process (PPP), which we use to generate rigorous collision probabilities for a robot body moving through the scene. Leveraging this expression for collision probability, we develop a fast method for online trajectory generation through NeRF scenes, which, offline, distills the NeRF density into a voxel-based representation of collision probability called the PURR.   Using the PURR we present an algorithm to plan trajectories represented as Bézier splines that guarantee a robot traversing the spline does not exceed a user-defined maximum collision probability. In numerical experiments, we show our proposed method generates safer and more efficient paths than a state-of-the-art method \cite{adamkiewicz2022vision} for trajectory planning through NeRFs, and also give more well-behaved and more user interpretable results than a baseline planer that uses a threshold on the NeRF density as a proxy for collision probability. We also demonstrate that our pipeline can run in real-time.

This work opens numerous directions for future work. Since our entire pipeline (both PURR generation and trajectory optimization) can run at real-time rates, our planner could be combined with a NeRF-based state estimator (e.g., \cite{loc-nerf, nice-slam}) to perform active exploration or ``next-best-view planning'' on NeRFs, allowing a robot to autonomously explore a novel environment using only onboard vision. Another interesting direction is to consider modifications to the NeRF rendering using insights gleaned from the PPP interpretation introduced in this work. In particular, considering pyramidal or conic ``beams'' instead of rectangular prisms could prove helpful for improving NeRF training on unbounded scenes or to reduce aliasing. Finally, the probabilistic collision framework developed here could have interesting applications to problems like differentiable simulation of rigid bodies represented as NeRFs \cite{cleach2022differentiable} or planning for problems like contact-rich manipulation and locomotion.

\section{Related Work}
\label{Sec:RelatedWork}
Here we review the related literature in robot planning and control with NeRF representations, other uses of NeRFs in robotics, planning and control for robots using traditional scene representations, and chance-constrained planning.

\subsection{Planning and Control using NeRFs}

Safety has been a largely unexplored topic in the NeRF literature, with only preliminary approaches being studied in simulation. The authors' previous work NeRF-Nav \cite{adamkiewicz2022vision} attempts to perform safe navigation of NeRF environments by penalizing the densities evaluated within the robot body modeled as a point cloud to discourage inter-NeRF penetration. \cite{tong2022enforcing} instead uses the predicted depth map at sampled poses to enforce step-wise safety using a control barrier function. The two methods are not at odds, as the philosophy of \cite{adamkiewicz2022vision} serves as a high-level planner that encourages non-myoptic behavior while \cite{tong2022enforcing} can be used as a safety filter for a myopic low-level controller interfacing directly with the system dynamics.

More specifically, NeRF-Nav \cite{adamkiewicz2022vision} adapts traditional trajectory optimization tools to plan trajectories through a NeRF environment. Collisions are discouraged with a penalty in the trajectory cost, but the probability of collision is not quantified or directly constrained. In this work, we instead rigorously quantify collision probabilities for a robot in a NeRF, and develop a trajectory planning method to satisfy user-defined chance constraints on collision. In addition, NeRF-Nav requires about 2 seconds for each online trajectory re-solve, while our proposed method requires about $.02$ sec per online trajectory resolve on similar computing hardware.

\subsection{Other Uses of NeRFs in Robotics}
Some works have considered NeRFs as a 3D scene representation for robotic grasping and manipulation. For example, Dex-NeRF \cite{ichnowski2021dex} uses NeRF-rendered depth images to obtain higher-quality grasps for a robot manipulator than using a depth camera. Similarly, \cite{yen2022nerfsupervision} uses dense object descriptors supervised with a NeRF model  for robot gasping.

Some works have also considered SLAM and mapping using a NeRF map representation. The papers  \cite{sucarIMAPImplicitMapping2021}, \cite{yen2021inerf} use the photometric error between rendered and observed images to simultaneously optimize the NeRF weights and the robot/camera trajectory. The approach in \cite{nice-slam} uses a grid-interpolation-decoder NeRF architecture in a similar SLAM pipeline. The work \cite{nerf-slam} proposes a combination of an existing visual odometry pipeline for camera trajectory estimation together with online NeRF training for the 3D scene.  NeRFs have also been used for tracking the pose of a robot using an on-board camera and IMU.  For example, \cite{yen2021inerf} finds a single camera pose from a single image and a pre-trained NeRF model, and \cite{adamkiewicz2022vision} proposes a nonlinear optimization-based filter for tracking a trajectory of an on-board camera using a sequence of images and a pre-trained NeRF.  \cite{loc-nerf} approaches a similar problem using a particle filter instead of a nonlinear optimization-based filter.

Other papers have considered active view planning for NeRFs. \cite{active-nerf} treats the radiance value of the NeRF as Gaussian distributed random variables, and performs Bayesian filtering to find the next best view. \cite{nerfensembles} uses disagreement among an ensemble of NeRFs to choose the next best camera view. Similarly, \cite{densitynerf} uses ensembles in a next best view strategy while also adding ray-termination densities to the information gain metric. \cite{snerf} uses variational inference to train a probability distribution over NeRFs for next best view selection. \cite{activermap} considers a full informative trajectory planning pipeline for a robot moving through a NeRF. in contrast to our work here, they do not focus on the safety of the trajectories or on quantifying collision probabilities.

Of course, many of these works would not be applicable to robotics if they were not real-time. \cite{mueller2022instant}, \cite{plenoxels}, \cite{plenoctrees} have made massive performance gains to train NeRFs in real-time. Moreover, NeRFs must be able to capture reality as well. They are known to suffer in quality when reconstructing rich, real environments with different length scales. \cite{mipnerf}, \cite{mipnerf360} attempts to fix this issue by extrapolating over the entire camera frustrum rather than a ray. Discussion on how their work relates to our results are in \ref{Sec:PPP}.

\subsection{Planning and Control with Onboard Sensing}
Planning and control onboard real hardware has already yielded a large amount of literature. Typically these works present reactive control schemes \cite{perceptionaware}, using the sensed depth directly to perform collision checking in real-time. These methods typically are either myopic, reasoning only locally about the scene. An alternate approach is to construct a map of the environment using the depth measurements. Often a Signed Distance Field (SDF) is constructed from depth data \cite{voxblox}, \cite{fiesta}, which in this work is encoded within voxels. \cite{fiesta} also integrates their system onboard a quadrotor to validate their method. Such a representation is typical in dynamic robotic motion planning, providing fast collision checking and gradients in planning.

\subsection{Chance-constrained Planning}
There exists large literature on trajectory planning for robots that seeks to impose constraints on the probability of collision when the underlying scene geometry is unkown; this approach is known as chance-constrained planning. This approach models the robot state as stochastic, while the parameters defining the constraint (i.e., the map) are known deterministically.  Some works do consider uncertainty in both the map and the robot state, but they typically rely on a linear system or Gaussian noise assumption to make computation convex or analytical and efficient to solve. \cite{toitCC} assumes Gaussian-distributed obstacle states, and approximates the collision probability as constant over the robot body (suitable only for small robots). \cite{blackmoreCC}
encodes the probability of collision with faces of polytopic obstacles as a linear constraint, but the resulting trajectory optimization is a combinatorial problem, making it difficult to solve quickly. \cite{zhuCC} incorporates this linear probabilistic constraint into RRT. \cite{ludersCC} again uses this linear constraint in an MPC framework with nonlinear dynamics, executed on real hardware with dynamic obstacles and extended to a multi-agent context.  None of these methods consider NeRF environment models, which is our focus here.

\section{Relation between density and PPP field}

\section{Integration over Trilinearly Interpolated Cells}\label{app:trilinear_analytic}

For a trilinearly interpolated density over a cell $v_{ijk}$ given by (\ref{trilinear}) with local coordinates $(x, y, z) \in $ ($\left[a_x, b_x \right], \left[a_y, b_y \right], \left[a_z, b_z \right]$), the volume integral over the cell can be computed analytically as:

\begin{equation}
\begin{split}
\int_{a_x}^{b_x}\int_{a_y}^{b_y}\int_{a_z}^{b_z} \rho(x) dxdydz  &=  (b_x - a_x)(b_y - a_y)(b_z - a_z) \\
    & + \frac{c_2}{2}(b_y - a_y)(b_z - a_z)(b_x^2 - a_x^2)\\
    & +  \frac{c_3}{2}(b_x - a_x)(b_z - a_z)(b_y^2 - a_y^2)\\
    & + \frac{c_4}{2}(b_x - a_x)(b_y - a_y)(b_z^2 - a_z^2)\\
    & + \frac{c_5}{4}(b_z - a_z)(b_x^2 - a_x^2)(b_y^2 - a_y^2)\\
    & + \frac{c_6}{4}(b_x - a_x)(b_y^2 - a_y^2)(b_z^2 - a_z^2)\\
    & + \frac{c_7}{4}(b_y - a_y)(b_x^2 - a_x^2)(b_z^2 - a_z^2)\\
    & + \frac{c_8}{8}(b_x^2 - a_x^2)(b_y^2 - a_y^2)(b_z^2 - a_z^2)
\end{split}
\end{equation}
where $c_i$ are the coefficients of the trilinear interpolation.

\section{Probabilistic Collision with State Uncertainty}\label{app:state_uncertain_purr}
\begin{figure}[t]
 \centering
 \includegraphics[width=0.49\textwidth]{figures/explainers/uncertain.png}
 \caption{(a) Bounded state uncertainty ellipse, (b) Dilation of obstacle reasoning only on the uncertainty bounds (equivalent to \texttt{MaxPool3D} operation), (c) Inflation of obstacle by reasoning over the distribution (equivalent to \texttt{Conv3D} operation). The black circle in (b) and (c) denotes the true obstacle.}
 \label{fig:uncertainty_explainer}
\end{figure}

The results of \ref{coll_prob} can be extended to account for state uncertainty in the agent body. The extent of $B(\mathcal{T})$ is a function of the pose random variable. The collision probability with state uncertainty is a marginalization over the support of the pose distribution. Explicitly,

\begin{equation}
\begin{split}\label{uncertain_margin}
Pr(N(B(\mathcal{T})) = 0) & = \int_S Pr(N(B(\mathcal{T})) = 0 \;|\; \mathcal{T})\; p(\mathcal{T}) d\mathcal{T}\\
& = \int_S \exp\left[ -\int_{B(\mathcal{T})} \frac{\rho(x)}{wh} \; dx \right] p(\mathcal{T}) d\mathcal{T},
\end{split}
\end{equation}

where $S$ is the support.

Due to the voxel nature of the PURR, state uncertainty can be incorporated into the representation cheaply. We first assume that the state uncertainty distribution is rotationally invariant (i.e. $\mathbf{p}$ is the only random variable) and that the support $S$ is parametrized by its mean $\mu$. We voxelize our state uncertainty equation (\ref{uncertain_margin})

\begin{equation}
\begin{split}\label{uncertain_voxel}
\bar{\mathbb{I}}_r(v_{ijk}, \mu) & = 1 - \sum_{v_{lmn} \in \mathbb{S}(v_{ijk})} \exp\left[-\mathbb{I}_r(v_{lmn})\right] \bar{p}(v_{lmn})\\
& \geq 1 - \int_S \exp\left[ -\int_{B(\mathcal{T})} \frac{\rho(x)}{wh} dx \right] p(\mathbf{p}) d\mathbf{p},
\end{split}
\end{equation}
where $\mathbb{S}$ is the voxelized support of the state and $\bar{p}(v_{lmn})$ is the accumulated density (i.e. discrete probability) within voxel $v_{lmn}$
\begin{equation}
    \bar{p}(v_{lmn}) = \int_{v_{lmn}} p(\mathbf{p}) d\mathbf{p}.
\end{equation}
This operation is similar to the creation of the robot kernel, replacing the binary values with values of discrete probability. The operation is illustrated in Fig. \ref{fig:uncertainty_explainer}c. Although in theory $\mathbf{p}$ can have unbounded support, in practice a probability cutoff is necessary so that the kernel used in \texttt{Conv3D} is finite.

Note that we can account for bounded uncertainty without reasoning about the distribution by simply selecting the cell with the maximum \emph{robot} intensity  in $\mathbb{S}$. This is a \texttt{MaxPool3D} operation using $\mathbb{S}$ as the kernel (Fig. \ref{fig:uncertainty_explainer}b).
\end{appendices}