# Wireless Point Cloud Transmission

Chenghong Bian, Yulin Shao, , and Deniz Gündüz The authors are with the
Department of Electrical and Electronic Engineering, Imperial College London,
London SW7 2AZ, U.K. (e-mail: {c.bian22,y.shao,d.gunduz}@imperial.ac.uk).

###### Abstract

3D point cloud is a three-dimensional data format generated by LiDARs and
depth sensors, and is being increasingly used in a large variety of
applications. This paper presents a novel solution called SEmantic Point cloud
Transmission (SEPT), for the transmission of point clouds over wireless
channels with limited bandwidth. At the transmitter, SEPT encodes the point
cloud via an iterative downsampling and feature extraction process. At the
receiver, SEPT reconstructs the point cloud with latent reconstruction and
offset-based upsampling. Extensive numerical experiments confirm that SEPT
significantly outperforms the standard approach with octree-based compression
followed by channel coding. Compared with a more advanced benchmark that
utilizes state-of-the-art deep learning-based compression techniques, SEPT
achieves comparable performance while eliminating the cliff and leveling
effects. Thanks to its improved performance and robustness against channel
variations, we believe that SEPT can be instrumental in collaborative sensing
and inference applications among robots and vehicles, particularly in the low-
latency and high-mobility scenarios.

###### Index Terms:

Joint source-channel coding, neural networks, point cloud, semantic
communication.

##  I Introduction

3D point clouds are collections of three-dimensional data points and their
associated attributes, such as color, temperature, and normals [1, 2, 3, 4,
5]. Generated through technologies such as light detection and ranging
(LiDAR), depth camera, and structured light scanning, point clouds are non-
ordered and non-uniformly distributed within space.

Wireless transmission plays a vital role in facilitating the mobility and
accessibility of 3D point clouds, empowering industries and applications
reliant on this expressive data format. Examples include autonomous driving,
medical imaging, augmented reality (AR), robotics, etc. However, it is
essential to acknowledge and address the inherent challenges that arise in
wireless communication, including potential data loss, latency, and bandwidth
limitations. These constraints necessitate a careful and dedicated design of
wireless technologies to meet the specific requirements of point cloud
transmission.

The standard approach for point cloud transmission consists of four main steps
[1]: octree decomposition, quantization, entropy coding, channel coding and
modulation. Octree is a canonical representation of point clouds. It
recursively partitions the space into eight equal sized octants or cubes, and
each node of the octree contains a point or a set of points in the
corresponding cube [2]. The standard approach faces several challenges in
achieving accurate and reliable transmission of 3D point cloud data:

  * •

Inefficient feature extraction. The octree representation cannot efficiently
extract contextual features from the 3D point cloud data and does not yield
good energy compaction [6]. This can result in a large dynamic range during
quantization.

  * •

The cliff and leveling effects. Two inherent issues of digital transmission
are the cliff and leveling effects [7, 8]. The cliff effect is characterized
by a sharp decline in transmission rate when the channel quality falls below a
certain threshold. The leveling effect, on the other hand, refers to the
phenomenon that the transmission rate fails to improve despite an improvement
in the channel quality, unless the coding rate and modulation order are
reconfigured adaptively.

Figure 1: The detailed architecture of SEPT with an encoder $h(\cdot)$ and a
decoder $g(\cdot)$. For clarity, we label the dimension information of the
point cloud after each processing module. Take $(\frac{N}{4},d_{f})$ after the
first point transformer at the encoder for an example, $\frac{N}{4}$ is the
number of points after the point transformer and $d_{f}$ is the dimension of
feature vectors.

Overcoming the above challenges requires the development of more efficient
feature extraction modules and communication protocols. In this paper, we
leverage the recent advances in deep joint source-channel coding (DeepJSCC)
[7] and develop a deep learning-based encoding and decoding framework for
wireless point cloud transmission. Our main contributions can be summarized as
follows:

  1. 1.

We present SEPT (SEmantic Point cloud Transmission), a tailored framework for
efficient delivery of 3D point cloud over additive white Gaussian noise (AWGN)
channels. To the best of our knowledge, this is the first work to utilize the
autoencoder approach in designing communication systems specifically for point
cloud transmission.

  2. 2.

To efficiently extract semantic features and avoid the cliff and leveling
effects, we develop novel DeepJSCC encoder and decoder architectures for 3D
point cloud: At the transmitter, SEPT encodes the point cloud directly into a
latent vector without voxelization. A flexible power normalization that
judiciously assigns power to different point clouds is applied. At the
receiver, as opposed to feeding the noisy latent vector directly into the up-
sampling layer, we introduce a refinement layer that uses the Point
Transformer[5] as the backbone to first denoise the latent vector. Finally,
offset-based up-sampling layers are employed for point cloud reconstruction.

  3. 3.

Extensive simulations are conducted to verify the reconstruction performance
of SEPT. Comparisons with the octree-based digital scheme demonstrate
significant performance gains achieved by SEPT. When compared with a more
advanced benchmark that combines state-of-the-art deep learning-based
compression [9] with Polar code, SEPT shows comparable reconstruction
performance while simultaneously eliminating the cliff and leveling effects.

Related work: There have been many efforts in processing and understanding 3D
point clouds using deep learning. The authors in [4] proposed PointNet that
uses permutation-invariant operations, such as pointwise multilayer
perceptrons (MLPs) and max-pooling, to extract features for point cloud
classification and segmentation. The follow-up works improved the performance
by using more advanced operations such as 3D convolution [10] and self-
attention [5, 11, 12]. The most related line of work to ours is point cloud
compression, for which different deep learning-based techniques have been
proposed recently [2, 13, 14, 9]. Among them, [2] used deep neural networks to
predict the occupancy probability for a certain node in the octree, exploiting
the information from its parent node and sibling nodes. Then, an entropy model
is used to generate the bit stream. Ref. [14] used multiple KPconv [10] and
downsampling layers to progressively reduce the number of points and extract
information from the points of previous layers, and an offset-based up-
sampling algorithm was proposed to reconstruct the point clouds at the
decoder. The authors in [9] used the point cloud transformer [11] as the
backbone to enhance the compression performance.

There is also growing interest in utilizing DeepJSCC to develop semantic
communication systems [7, 15]. It is shown in [7] that by end-to-end
optimizing the DeepJSCC system, both the cliff and leveling effects can be
eliminated. With such merits, researchers have actively applied DeepJSCC to
different wireless channels, e.g., multi-path fading [16], MIMO [17], and
relay channels [18], as well as different data sources, e.g., text [19], image
[7, 20], speech [21, 22], video [23, 24], or even wireless channel state
information [25].

For different channels and data sources, it is crucial to employ appropriate
methods to harness their characteristics to maximize the potential of
DeepJSCC. While convolutional neural network (CNN) based autoencoders have
been successfully applied to many of these sources in the aforementioned
works, three-dimensional point clouds constitute a much more challenging data
source as they are unstructured and can be represented in an arbitrary
coordinate system. Moreover, the points can be presented in any arbitrary
order, which makes it difficult to apply any specified filter to capture the
structure among neighbouring points. The main objective of semantic-oriented
joint source-channel coding approach to wireless signal delivery is to extract
the relevant features of the signal for the specified task, and to map similar
features to similar channel inputs so that the reconstruction is robust
against channel noise. However, the lack of structure in point clouds makes it
highly challenging to apply CNN-based DeepJSCC techniques, despite their
recent success in the wireless transmission of image and video sources.

##  II System Model

(a)

(b)

(c)

Figure 2: The inner architectures of (a) the downsampling module, (b) the
point transformer layer, and (c) the $\ell$-th block of the offset-based up-
sampling module, $\ell\in[1,L]$.

We consider transmitting a 3D point cloud over an AWGN channel. A point cloud
can be expressed as $\mathcal{P}=(\bm{X},\bm{F})$, where
$\bm{X}\triangleq\\{\bm{x}_{i}\in\mathbb{R}^{3}\\}$, $i\in[N]$, is a set of
$N$ points in space, and
$\bm{F}\triangleq\\{\bm{f}_{i}\in\mathbb{R}^{d}\\},i\in[N]$, is a set of
features associated with each point in $\bm{X}$. In particular, this paper
considers point clouds with no input attributes111Nevertheless, the point
clouds in the intermediate layers of SEPT can have non-trivial
attributes/features. For example, the neighboring information is contained in
the attributes of the downsampled points. and focuses on transmitting only the
coordinates $\bm{X}$. Following the convention, we set the input attributes
$\bm{F}$ to an all-ones vector with $d=1$.

The detailed architecture of SEPT is presented in Fig. 1, where we denote the
DeepJSCC encoder and decoder by $h(\cdot)$ and $g(\cdot)$, respectively. In
the big picture, the encoder $h(\cdot)$ first maps the input 3D point cloud
$\mathcal{P}$ to a latent vector $\bm{\tilde{z}}\in\mathbb{R}^{n}$ where $n$
is the available channel bandwidth. Then, we power normalize the latent vector
to obtain a codeword $\bm{{z}}\in\mathbb{R}^{n}$ and transmit it to the
receiver via discrete-time analog transmission (DTAT) [26]. In particular,
instead of imposing a stringent power constraint such that the power of each
codeword $\bm{z}$ is bounded by a power budget $nP/2$, we adopt a more
flexible average power constraint [27]: $\mathbb{E}||\bm{z}||^{2}_{2}\leq
nP/2,$ whereby the power of codewords associated with different point clouds
can be adjusted judiciously. To achieve this, we record the moving mean $\mu$
and deviation $\sigma$ of $\bm{\tilde{z}}$ during the training phase. Then, in
the inference phase, the latent vector $\tilde{\bm{z}}$ is normalized using
$(\mu,\sigma)$, yielding

$\bm{z}=(\bm{\tilde{z}}-\mu)/\sigma.$ (1)

It is worth noting that the codeword $\bm{z}$ is converted to a complex
vector, $\bm{z}^{\prime}\in\mathbb{C}^{n/2}$, when passing through the complex
AWGN channel. The channel use per point (CPP) is given by $\frac{n}{2N}$.

At the receiver, the received signal $\bm{{y}}^{\prime}\in\mathbb{C}^{n/2}$ is
a noisy version of $\bm{{z}}^{\prime}$:

$\displaystyle\bm{{y}}^{\prime}=\bm{{z}}^{\prime}+\bm{w},$ (2)

where $\bm{w}\in\mathbb{C}^{n/2}$ denotes a complex AWGN vector with
independent and identically distributed elements,
$\bm{w}\sim\mathcal{CN}(0,N_{0})$. The channel signal-to-noise ratio (SNR) is
defined as $\mathrm{SNR}=10\log_{10}{\frac{P}{N_{0}}}$. Without loss of
generally, we assume $P=1$ in the sequel. Upon receiving $\bm{{y}}^{\prime}$,
we first convert it to a real vector $\bm{y}\in\mathbb{R}^{n}$ and feed it
into the decoder $g(\cdot)$ to obtain a reconstructed point cloud
$\hat{\mathcal{P}}$.

##  III Methodology

This section details our design of the encoder and decoder functions using
neural network architectures, and explains how features are extracted from the
original point cloud $\mathcal{P}$ via downsampling and self-attention layers
and how the point cloud $\hat{\mathcal{P}}$ is reconstructed from the noisy
latent vector via refinement and up-sampling layers.

###  III-A SEPT encoder

The encoder of SEPT consists of three main modules: downsampling, self-
attention, and max pooling, as shown in Fig. 1.

Downsampling. The objective of the downsampling module is to reduce the number
of points in the input point cloud. Let
$\mathcal{P}_{1}=(\bm{X}_{1},\bm{F}_{1})$ and
$\mathcal{P}_{2}=(\bm{X}_{2},\bm{F}_{2})$ denote the input and output point
clouds of a downsampling module, respectively, where
$\bm{X}_{2}\subset\bm{X}_{1}$. In particular,

  * •

In order to achieve a more representative point cloud, it is crucial to
disperse the selected points, i.e., $\bm{X}_{2}$, as widely as possible,
ensuring sufficient coverage across $\bm{X}_{1}$.

  * •

The clipped points of $\mathcal{P}_{1}$ will be embedded into the features of
$\mathcal{P}_{2}$ to facilitate reconstruction at the receiver.

To the above ends, SEPT uses the farthest point sampling (FPS) algorithm to
generate $\bm{X}_{2}$, as shown in Fig. 2(a). To generate the feature vector
of the $i$-th point in $\bm{X}_{2}$, denoted by
$\bm{f}^{(2)}_{i}\in\mathbb{R}^{d_{f_{2}}}$, we first find its $k$-nearest
neighbors within a given radius $r$ in $\bm{X}_{1}$ and denote them by
$\mathbb{N}(i)$.222If the $i$-th ($i<k$) neighbor has a distance larger than
$r$ from the sampled point, we will use the nearest neighbor to replace it.
Then, we concatenate the feature vectors with their coordinates of the points
in $\mathbb{N}(i)$ and organise them into a tensor, denoted by
$\bm{T}_{\mathbb{N}(i)}\in\mathbb{R}^{(d_{f_{1}}+3)}\times k\times N_{1}$
where $N_{1}$ denotes the cardinality of $\bm{X}_{1}$, and feed this tensor to
a 2D convolutional layer followed by batch normalization, ReLU, and max
pooling. In each downsampling module, we set the cardinality of $\bm{X}_{2}$
to be $1/4$ of that of $\bm{X}_{1}$, i.e., $|\bm{X}_{2}|=|\bm{X}_{1}|/4$.

Self-Attention. In SEPT, each downsampling module is followed by a self-
attention module [5], a point cloud processing technique that is capable of
extracting rich neighboring information. Denote the input and output point
clouds of the self-attention layer by
$\mathcal{P}_{1}=(\bm{X}_{1},\bm{F}_{1})$ and
$\mathcal{P}_{2}=(\bm{X}_{1},\bm{F}_{2})$, respectively. As shown in Fig.
2(b), the self-attention layer refines the features of each point in
$\mathcal{P}_{1}$. The inner operations can be written as

$\bm{f}^{(2)}_{i}\\!=\\!\\!\sum_{j\in\mathbb{N}(i)}\\!\text{Softmax}\big{(}\gamma(\phi(\bm{f}^{(1)}_{i})-\phi(\bm{f}^{(1)}_{j}))+\delta\big{)}\odot(\alpha(\bm{f}^{(1)}_{j})+\delta),$

where $\bm{f}^{(2)}_{i}$ denotes the refined feature vector of the $i$-th
point; $\gamma,\phi,\alpha:\mathbb{R}^{d_{f}}\rightarrow\mathbb{R}^{d_{f}}$
are realized by MLPs; $\delta=\theta(\bm{x}_{i}-\bm{x}_{j})$ is the positional
information and $\theta:\mathbb{R}^{3}\rightarrow\mathbb{R}^{d_{f}}$ is an MLP
layer for positional embedding; $\odot$ denotes the element-wise product. That
is, we adopt vector attention [5], as opposed to the standard scalar dot-
product attention used in language and vision transformer models, for better
performance.

As shown in Fig. 1, after two consecutive downsampling and self-attention
module pairs, the final downsized point cloud is obtained, which we denote by
$\mathcal{P}^{*}=(\bm{X}^{*},\bm{F}^{*})$, where
$\bm{X}^{*}\in\mathbb{R}^{(N/16)\times 3}$ and
$\bm{F}^{*}\in\mathbb{R}^{(N/16)\times n}$. We emphasize that the features
$\bm{F}^{*}$ are generated by neural networks and can be optimized to be
robust to noise, thanks to end-to-end learning. On the other hand, the
coordinates $\bm{X}^{*}$ are obtained from FPS and are susceptible to noise.
Our empirical results indicate that $\bm{X}^{*}$ has to be transmitted to the
receiver reliably via digital communications. Failure to do so results in a
substantial degradation in the reconstruction performance of the point cloud.
Digital transmission of $\bm{X}^{*}$, however, results in two problems: 1) the
cliff and leveling effects; 2) excessive channel usage (detailed later in
Section IV-B). In this context, SEPT eliminates the need for coordinate
($\bm{X}^{*}$) transmission and instead focuses solely on transmitting the
features ($\bm{F}^{*}$) to the receiver. To be precise, SEPT learns to encode
the global features in $\bm{F}^{*}$ and the decoder is trained to reconstruct
the entire point cloud from the global features without the aid of the
coordinates. By doing so, SEPT significantly reduces the amount of data that
needs to be transmitted, leading to more efficient and streamlined
communication.

Max Pooling. The last step at the encoder is to transform
$\bm{F}^{*}\in\mathbb{R}^{(N/16)\times n}$ to the latent vector
$\tilde{\bm{z}}\in\mathbb{R}^{n}$. A natural solution is to use an MLP for
each $\bm{f}_{i}^{*}\in\mathbb{R}^{n}$. In contrast, SEPT applies max pooling
over the $N/16$ points to generate the $n$-dimensional vector $\tilde{\bm{z}}$
where $n$ is the available channel bandwidth. The advantage of max pooling
will be demonstrated in Section IV-B via an ablation study.

###  III-B SEPT decoder

The decoder of SEPT consists of two modules: latent reconstruction and
refinement, and offset-based up-sampling.

Latent Reconstruction and Refinement. The latent reconstruction module takes
the noisy latent vector as input to reconstruct $\mathcal{P}^{*}$. As shown in
Fig. 1, given the received signal $\bm{y}$, we first use a TransConv layer,
which is essentially a 1D deconvolution with a unit stride, to generate the
initial estimate of $\bm{F}^{*}$, denoted by
${\bm{F}}^{\prime}\in\mathbb{R}^{(N/16)\times n}$. Then, we employ a
coordinate reconstruction layer
$\Psi^{\prime}:\mathbb{R}^{n}\rightarrow\mathbb{R}^{3}$, which is composed of
MLP layers and a ReLU function, operates on each row of ${\bm{F}}^{\prime}$ to
generate an initial estimate of the coordinates:

$\displaystyle{\bm{X}}^{\prime}=\Psi^{\prime}({\bm{F}}^{\prime}),$ (3)

where ${\bm{X}}^{\prime}\in\mathbb{R}^{N/16\times 3}$. The initial estimates
$({\bm{X}}^{\prime},{\bm{F}}^{\prime})$ can be erroneous due to noise.
Therefore, we further use a self-attention module, denoted as SA, to refine
the features:

$\displaystyle{\bm{F}}^{\prime\prime}=\text{SA}({\bm{X}}^{\prime},{\bm{F}}^{\prime}).$
(4)

Next, a new coordinate reconstruction layer, $\Psi^{\prime\prime}$, is applied
to ${\bm{F}}^{\prime\prime}$ to produce a refined estimation of coordinates,
${\bm{X}}^{\prime\prime}$. Our refinement module is shown to be very effective
in denoising the coordinates and features. An ablation study will be provided
in Section IV-B.

Offset-Based Up-Sampling. Finally, we employ an offset-based up-sampling
module [14] on $({\bm{X}}^{\prime\prime},{\bm{F}}^{\prime\prime})$ for point
cloud reconstruction. For the $i$-th point in the input point cloud, whose
coordinates and features are denoted by $(\bm{x}_{i},\bm{f}_{i})$, this module
generates $L$ new points
$\\{(\bm{x}_{i}^{\ell},\bm{f}_{i}^{\ell}),\ell\in[1,L]\\}$ as:

$\displaystyle\bm{x}_{i}^{\ell}$ $\displaystyle=\bm{x}_{i}+s\cdot
O_{\ell}(\bm{f}_{i}),$ (5) $\displaystyle\bm{f}_{i}^{l}$
$\displaystyle=G_{\ell}(\bm{f}_{i}),$ (6)

where $O_{\ell}:\mathbb{R}^{d_{f}}\rightarrow[-1,1]^{3}$ is an MLP layer
followed by a $\mathrm{tanh}$ function that aims to generate an offset vector;
$G_{\ell}$ is comprised of MLPs and a ReLU function that maps the input
feature to a new one with the same dimension. In particular, $s$ is a scaling
factor for the offsets. Compared with [14], SEPT uses a relatively large
$s=0.1$ to give the up-sampling module more freedom for better performance,
considering the additional noise introduced by the wireless channel. The
detailed architectures for $O_{\ell}$ and $G_{\ell}$ are shown in Fig. 2(c).

In SEPT, we use two up-sampling modules, and $L$ is set to $4$ in each module.
Denoting by $\hat{\mathcal{P}}=(\hat{\bm{X}},\hat{\bm{F}})$ the final output
of the up-sampling modules, the Chamfer distance between ${\bm{X}}$ and
$\hat{\bm{X}}$, denoted by $d^{2}_{cd}({\bm{X}},\hat{\bm{X}})$, is used as the
loss function:

$\displaystyle\mathcal{L}_{\text{CD}}=\frac{1}{N}\sum_{\bm{x}\in\mathcal{P}}\min_{\bm{y}\in\hat{\mathcal{P}}}||\bm{x}-\bm{y}||^{2}_{2}+\frac{1}{N}\sum_{\bm{y}\in\hat{\mathcal{P}}}\min_{\bm{x}\in{\mathcal{P}}}||\bm{y}-\bm{x}||^{2}_{2}.$
(7)

##  IV Numerical Experiments

This section presents the results of our numerical experiments to evaluate the
reconstruction performance of SEPT. We consider the point cloud data from
ShapeNet [28], which contains about $51000$ different shapes, and we sample
each point cloud to $N=2048$ points using the FPS algorithm. In both the SEPT
encoder and decoder, the dimension of the intermediate attributes is set to
$d_{f}=256$ and the number of neurons in the MLPs of the coordinate
reconstruction layer is set to $128$. During training, we adopt the Adam
optimizer with a varying learning rate, which is initialized to 0.001 and
reduced by a factor of $0.5$ every $20$ epochs. We set the number of epochs to
$200$ and the batch size to $32$. Two conventional peak signal-to-noise ratio
(PSNR) measures [29], D1 and D2, are adopted to evaluate the reconstruction
quality. Specifically, $\text{D1}(\mathcal{A},\mathcal{B})$ measures the
average point-to-point geometric distance between each point in point cloud
$\mathcal{A}$ and its nearest neighbor in point cloud $\mathcal{B}$. To be
precise, we first calculate the mean squared error,
$e_{\mathcal{A},\mathcal{B}}^{D1}$:

$\displaystyle e_{\mathcal{A},\mathcal{B}}^{D1}$
$\displaystyle=\frac{1}{N}\sum_{\bm{a}_{i}\in\mathcal{A}}||\bm{a}_{i}-\bm{b}_{k}||_{2}^{2}$
$\displaystyle k$
$\displaystyle=\operatorname*{arg\,min}_{j\in[N]}{||\bm{a}_{i}-\bm{b}_{j}||_{2}^{2}}.$
(8)

then, D1 is calculated as [29]:

$\text{D1}(\mathcal{A},\mathcal{B})=10\frac{3\gamma^{2}}{\max(e_{\mathcal{A},\mathcal{B}}^{D1},e_{\mathcal{B},\mathcal{A}}^{D1})},$
(9)

where factor $3$ in the numerator is due to the 3D coordinates used in the
representation, and the peak, $\gamma$, is set to unity due to the fact that
the input points are normalized within the range $[0,1]$. Similarly, D2
evaluates the point-to-plane distance between $\mathcal{A}$ and $\mathcal{B}$
and the error term for D2 is defined as:

$e_{\mathcal{A},\mathcal{B}}^{D2}=\frac{1}{N}\sum_{\bm{a}_{i}\in\mathcal{A}}(\bm{a}_{i}-\bm{b}_{k})\cdot\bm{n}_{i},$
(10)

where $\bm{n}_{i}$ is the normal vector corresponding to $\bm{a}_{i}$ and
$\bm{b}_{k}$ is the nearest neighbor of $\bm{a}_{i}$. After obtaining (10), we
follow the same formula in (9) to calculate
$\text{D2}(\mathcal{A},\mathcal{B})$.

Table I: D1 performance (in dB) comparison between max pooling and linear projection to generate the channel inputs ($n=300$). $\mathrm{SNR}$ |  0 dB |  5 dB |  10 dB
---|---|---|---
Max Pooling |  34.33 |  35.27 |  35.63
Linear Projection |  26.41 |  30.22 |  31.27

(a) (b)

(c)

Figure 3: Reconstruction performances of SEPT: (a) $\&$ (b) the D1 and D2
performances, where $n=200$; (c) D1 versus the test SNR, where $n=50$ and the
SEPT model is trained with $\mathrm{SNR}_{\text{train}}=5$ dB.

###  IV-A The reconstruction performance

We first evaluate the reconstruction performance of SEPT with various CPP and
channel SNR values. Two separate source-channel coding schemes are considered
as benchmarks. For source coding, the first benchmark uses the standard
octree-based point cloud compression scheme: MPEG G-PCC [3]. The second
benchmark uses the state-of-the-art deep learning-based point cloud
compression scheme, which we name it as DPCC [9]. Both schemes are protected
by Polar codes with rate $\\{1/2,3/4\\}$ and modulated by BPSK, QPSK, or 16QAM
for transmission. We also provide the results for the DPCC delivered at finite
block length converse bound [30] for a block error rate of $\epsilon=10^{-3}$.

Figure 4: The reconstruction performance (D1) of the proposed SEPT with
respect to different number of channel uses $n$. We fix the $\text{SNR}=10$
dB.

The simulation results are presented in Fig. 3 (a) and (b), where we fix the
channel bandwidth to $n=200$. In the simulations, a specific SEPT model is
trained for each channel SNR value. As shown, SEPT is significantly better
than the separation based scheme with MPEG G-PCC333To obtain the results of
G-PCC, we use an average $n=860$ in the simulations. Despite the much larger
$n$ compared with that used in SEPT, MPEG G-PCC is still much worse than deep
learning-based schemes. This observation is also reported in [9].,
demonstrating its efficacy in feature extraction and robustness to channel
noise. SEPT also outperforms the separation-based scheme with DPCC [9],
especially in the low-SNR regime. Note that DPCC can achieve comparable
performance to SEPT at certain SNRs, if proper coded modulation schemes are
selected.

Next, we show how SEPT eliminates the cliff and leveling effects. To this end,
Fig. 3(c) evaluates the performance of the SEPT model trained with a
$\mathrm{SNR}_{\text{train}}=5$ dB under various channel conditions,
$\mathrm{SNR}_{\text{test}}=\\{0,2.5,5,7.5,10\\}$ dB. For these simulations,
we set $n=50$. As shown, the single SEPT model trained with
$\mathrm{SNR}_{\text{train}}=5$ dB is robust to channel variations and
performs well under various test SNRs. Importantly, SEPT eliminates the cliff
and leveling effects, its performance degrades gracefully with the decrease in
the test SNR and improves when the test SNR increases, while the digital
benchmarks suffer from both the cliff and leveling effects if the modulation
order remains unchanged, as shown in Fig. 3(a) and (b).

The performance of the proposed SEPT with respect to different number of
channel uses $n$ are shown in Fig. 4. Two SNR values, $\\{0,10\\}$ dB, are
considered and we compare the SEPT with the DPCC baseline delivered at finite
length capacity. We can observe that D1 and D2 almost saturate when $n\geq
200$. This might be due to the max pooling operation at the transmitter
focuses more on the global features while the fine details may be lost.

We also provide a visualization of the reconstructed point clouds with $n=300$
and $\mathrm{SNR}\in\\{0,5\\}$ dB. As shown in Fig. 5, SEPT yields visually
pleasing results even when the $\mathrm{SNR}$ is as low as 0 dB.

###  IV-B Ablation study

This section performs ablation studies to evaluate different modules of SEPT.

Max pooling versus linear projection. As mentioned in Section III-A, to
generate $\tilde{\bm{z}}$ from the feature vectors
$\bm{F}^{*}\in\mathcal{R}^{N/16\times n}$, we can either use max pooling to
produce an $n$-dimensional vector or perform MLP layers on $\bm{F}^{*}$ to
generate an $N/16\times t$ matrix satisfying $Nt/16\approx n$. For comparison,
we set $n=300,t=3$ for the two schemes, and report the reconstruction
performance with $\mathrm{SNR}=\\{0,5,10\\}$ dB in terms of D1 and D2 in Table
I. It is confirmed that max pooling is superior to a linear projection.

The refinement networks at the decoder. The initial estimates of both the
coordinates and the features, $\\{\bm{{X}}^{\prime},\bm{{F}}^{\prime}\\}$,
obtained from the 1D deconvolution layer are noisy, and SEPT uses an
additional self-attention layer and a coordinate estimation layer to refine
these coordinates and features. In this simulation, we show the effectiveness
of the proposed refinement neural network by comparing the Chamfer distance
(7) between $\\{{\mathcal{P}}^{\prime\prime},{\mathcal{P}}\\}$ with that of
$\\{{\mathcal{P}}^{\prime},{\mathcal{P}}\\}$, where $\mathcal{P}$ is the
original point cloud. Simulations are performed with $n=300$ and
$\mathrm{SNR}=0$ dB, and we have
$\mathcal{L}_{\text{CD}}({\bm{X}}^{\prime},{\bm{X}})=0.76$ and
$\mathcal{L}_{\text{CD}}({\bm{X}}^{\prime\prime},{\bm{X}})=0.018$ illustrating
that the refinement layer provides a much more accurate reconstruction.

Transmitting downsampled coordinates. As stated in Section III-B, SEPT only
transmits the features $\bm{F}^{*}$ of the downsized point cloud, but not the
coordinates $\bm{X}^{*}$. For a comprehensive understanding of SEPT, we
further explore a hybrid transmission scheme, where $\bm{X}^{*}$ is
transmitted in the digital fashion while $\bm{F}^{*}$ is transmitted with
DTAT. Note that this hybrid transmission strategy will still suffer from the
cliff and leveling effects.

In this simulation, we consider a $\mathrm{SNR}=10$ dB and $n=200$. For the
hybrid scheme, we first downsample the original point cloud to
$\mathcal{P}^{*}$ with $64$ points and then perform max pooling to generate a
latent vector $\tilde{\bm{z}}$ with $n=200$, as SEPT. Then, $\bm{X}^{*}$ is
quantized, and the quantized bits are transmitted using $1/2$-rate Polar code,
and 16QAM modulation. At $\mathrm{SNR}=10$ dB, this coded modulation scheme
achieves zero error probability for coordinate transmission. At the receiver,
the coordinate reconstruction layers are no longer needed, since accurate
coordinates are already available. The features are refined and the up-
sampling blocks are used to reconstruct the final point clouds with $N$
points. We observe a slight improvement in D1 by sending extra coordinate
information from $35.4$ dB to $36.2$ dB. However, even if we consider a
$16$-bit precision of the coordinates as in [14], the extra cost for
transmitting the coordinates is $3072$ bits, occupying $\approx 900$ excessive
complex channel uses (assuming capacity achieving codes), which is
unacceptable, given that we only have $n=200$.

Figure 5: Visualization of reconstructed point clouds. Left: original
$\mathcal{P}$; middle: $\hat{\mathcal{P}}$ with $n=300,\mathrm{SNR}=5$ dB;
right: $\hat{\mathcal{P}}$ with $n=300,\mathrm{SNR}=0$ dB.

##  V Conclusion

Wireless transmission plays a pivotal role in enhancing the mobility and
accessibility of 3D point clouds. With limited bandwidth, the SEPT framework
proposed in this paper achieves efficient and robust wireless transmission of
3D point clouds, paving the way for realizing immersive user experiences in
the metaverse, or collaborative sensing in vehicular networks. Our study
highlights two key challenges that merit further investigation:

  * •

The potential for a hybrid scheme that transmits both point cloud coordinates
and features for improved performance, albeit at the expense of increased
bandwidth utilization. A direction worthy of exploration involves designing a
cost-effective hybrid scheme that strikes a balance between performance
enhancement and bandwidth efficiency.

  * •

Our findings indicate that the reconstruction performance reaches a saturation
point as the CPP increases. This implies that certain intricate details of the
point cloud are not fully preserved during feature extraction. To address this
issue, it is crucial to develop new encoding and decoding architectures that
effectively capture these fine details, enabling progressive performance
improvements with higher CPP values.

## References

  * [1] R. B. Rusu and S. Cousins, “3D is here: Point Cloud Library,” in _IEEE International Conference on Robotics and Automation_ , 2011.
  * [2] L. Huang, S. Wang, K. Wong, J. Liu, and R. Urtasun, “OctSqueeze: Octree-structured entropy model for LiDAR compression,” in _Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR)_ , June 2020.
  * [3] D. Graziosi, O. Nakagami, S. Kuma, A. Zaghetto, T. Suzuki, and A. Tabatabai, “An overview of ongoing point cloud compression standardization activities: Video-based (V-PCC) and geometry-based (G-PCC),” _APSIPA Transactions on Signal and Information Processing_ , vol. 9, pp. 1–13, 2020.
  * [4] C. R. Qi, H. Su, K. Mo, and L. J. Guibas, “Pointnet: Deep learning on point sets for 3D classification and segmentation,” in _Proceedings of the IEEE conference on computer vision and pattern recognition_ , 2017, pp. 652–660.
  * [5] H. Zhao, L. Jiang, J. Jia, P. H. Torr, and V. Koltun, “Point transformer,” in _Proceedings of the IEEE/CVF international conference on computer vision_ , 2021, pp. 16 259–16 268.
  * [6] P. de Oliveira Rente, C. Brites, J. Ascenso, and F. Pereira, “Graph-based static 3D point clouds geometry coding,” _IEEE Transactions on Multimedia_ , vol. 21, no. 2, pp. 284–299, 2018.
  * [7] E. Bourtsoulatze, D. B. Kurka, and D. Gündüz, “Deep joint source-channel coding for wireless image transmission,” _IEEE Trans. Cognitive Commun. Netw._ , vol. 5, no. 3, pp. 567–579, 2019.
  * [8] T. Fujihashi, T. Koike-Akino, T. Watanabe, and P. V. Orlik, “HoloCast+: Hybrid digital-analog transmission for graceful point cloud delivery with graph Fourier transform,” _IEEE Transactions on Multimedia_ , vol. 24, pp. 2179–2191, 2021.
  * [9] J. Zhang, G. Liu, D. Ding, and Z. Ma, “Transformer and upsampling-based point cloud compression,” in _Proceedings of the 1st International Workshop on Advances in Point Cloud Compression, Processing and Analysis_ , 2022, pp. 33–39.
  * [10] H. Thomas, C. R. Qi, J.-E. Deschaud, B. Marcotegui, F. Goulette, and L. J. Guibas, “Kpconv: Flexible and deformable convolution for point clouds,” in _Proceedings of the IEEE/CVF international conference on computer vision_ , 2019, pp. 6411–6420.
  * [11] M.-H. Guo, J.-X. Cai, Z.-N. Liu, T.-J. Mu, R. R. Martin, and S.-M. Hu, “Pct: Point cloud transformer,” _Computational Visual Media_ , vol. 7, pp. 187–199, 2021.
  * [12] C. Park, Y. Jeong, M. Cho, and J. Park, “Fast point transformer,” in _Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition_ , 2022, pp. 16 949–16 958.
  * [13] J. Wang, D. Ding, Z. Li, and Z. Ma, “Multiscale point cloud geometry compression,” in _2021 Data Compression Conference (DCC)_. IEEE, 2021, pp. 73–82.
  * [14] L. Wiesmann, A. Milioto, X. Chen, C. Stachniss, and J. Behley, “Deep compression for dense point cloud maps,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 2, pp. 2060–2067, 2021.
  * [15] D. Gündüz, Z. Qin, I. E. Aguerri, H. S. Dhillon, Z. Yang, A. Yener, K. K. Wong, and C.-B. Chae, “Beyond transmitting bits: Context, semantics, and task-oriented communications,” _IEEE J. Sel. Area Comm._ , 2022.
  * [16] M. Yang, C. Bian, and H.-S. Kim, “OFDM-guided deep joint source channel coding for wireless multipath fading channels,” _IEEE Transactions on Cognitive Communications and Networking_ , vol. 8, no. 2, pp. 584–599, 2022.
  * [17] H. Wu, Y. Shao, C. Bian, K. Mikolajczyk, and D. Gündüz, “Vision transformer for adaptive image transmission over MIMO channels,” in _IEEE International Conference on Communications (ICC)_ , 2023.
  * [18] C. Bian, Y. Shao, H. Wu, and D. Gunduz, “Deep joint source-channel coding over cooperative relay networks,” 2022.
  * [19] N. Farsad, M. Rao, and A. Goldsmith, “Deep learning for joint source-channel coding of text,” in _2018 IEEE International Conference on Acoustics, Speech and Signal Processing (ICASSP)_ , 2018, pp. 2326–2330.
  * [20] J. Dai, S. Wang, K. Tan, Z. Si, X. Qin, K. Niu, and P. Zhang, “Nonlinear transform source-channel coding for semantic communications,” _IEEE Journal on Selected Areas in Communications_ , vol. 40, no. 8, pp. 2300–2316, 2022\.
  * [21] Z. Weng, Z. Qin, X. Tao, C. Pan, G. Liu, and G. Y. Li, “Deep learning enabled semantic communications with speech recognition and synthesis,” _IEEE Transactions on Wireless Communications_ , pp. 1–1, 2023.
  * [22] T. Han, Q. Yang, Z. Shi, S. He, and Z. Zhang, “Semantic-preserved communication system for highly efficient speech transmission,” _IEEE Journal on Selected Areas in Communications_ , vol. 41, no. 1, pp. 245–259, 2023\.
  * [23] T.-Y. Tung and D. Gündüz, “DeepWiVe: Deep-learning-aided wireless video transmission,” _IEEE Journal on Selected Areas in Communications_ , vol. 40, no. 9, pp. 2570–2583, 2022.
  * [24] S. Wang, J. Dai, Z. Liang, K. Niu, Z. Si, C. Dong, X. Qin, and P. Zhang, “Wireless deep video semantic transmission,” _IEEE Journal on Selected Areas in Communications_ , vol. 41, no. 1, pp. 214–229, 2023.
  * [25] M. B. Mashhadi, Q. Yang, and D. Gündüz, “Cnn-based analog csi feedback in fdd mimo-ofdm systems,” in _IEEE International Conference on Acoustics, Speech and Signal Processing (ICASSP)_ , 2020, pp. 8579–8583.
  * [26] Y. Shao and D. Gunduz, “Semantic communications with discrete-time analog transmission: A PAPR perspective,” _IEEE Wireless Communications Letters_ , vol. 12, no. 3, pp. 510–514, 2022.
  * [27] E. Ozfatura, Y. Shao, A. G. Perotti, B. M. Popović, and D. Gündüz, “All you need is feedback: Communication with block attention feedback codes,” _IEEE Journal on Selected Areas in Information Theory_ , vol. 3, no. 3, pp. 587–602, 2022.
  * [28] A. X. Chang, T. Funkhouser, L. Guibas, P. Hanrahan, Q. Huang, Z. Li, S. Savarese, M. Savva, S. Song, H. Su _et al._ , “Shapenet: An information-rich 3D model repository,” _arXiv preprint arXiv:1512.03012_ , 2015.
  * [29] “Common test conditions for point cloud compression,” _ISO/IEC JTC1/SC29/WG11 MPEG output document N19084_ , Feb. 2020.
  * [30] Y. Polyanskiy, H. V. Poor, and S. Verdu, “Channel coding rate in the finite blocklength regime,” _IEEE Transactions on Information Theory_ , vol. 56, no. 5, pp. 2307–2359, 2010.


# Interactive-FAR:Interactive, Fast and Adaptable Routing for Navigation Among
Movable Obstacles in Complex Unknown Environments

Botao He ${}^{{\dagger}\,}$1, Guofei Chen ${}^{{\dagger}\,}$2, Wenshan Wang 2,
Ji Zhang* 2, Cornelia Fermuller1, and Yiannis Aloimonos 1 ${{\dagger}}$ Equal
contribution.1 Perception and Robotics Group, University of Maryland, MD
20742.2 Robotics Institute, Carnegie Mellon University, PA 15213-3890.Email:
<EMAIL_ADDRESS><EMAIL_ADDRESS>
###### Abstract

This paper introduces a real-time algorithm for navigating complex unknown
environments cluttered with movable obstacles. Our algorithm achieves fast,
adaptable routing by actively attempting to manipulate obstacles during path
planning and adjusting the global plan from sensor feedback. The main
contributions include an improved dynamic Directed Visibility Graph (DV-graph)
for rapid global path searching, a real-time interaction planning method that
adapts online from new sensory perceptions, and a comprehensive framework
designed for interactive navigation in complex unknown or partially known
environments. Our algorithm is capable of replanning the global path in
several milliseconds. It can also attempt to move obstacles, update their
affordances, and adapt strategies accordingly. Extensive experiments validate
that our algorithm reduces the travel time by $33\%$, achieves up to $49\%$
higher path efficiency, and runs faster than traditional methods by orders of
magnitude in complex environments. It has been demonstrated to be the most
efficient solution in terms of speed and efficiency for interactive navigation
in environments of such complexity. We also open-source our code in the docker
demo111Interactive-FAR: github.com/Bottle101/Interactive-FAR to facilitate
future research.

##  I Introduction

Figure 1:  Illustration of the interactive navigation through an unknown
environment. The colorful curve is the vehicle’s trajectory. The obstacles are
extracted as polygons in red, and the movable objects (orange pixels) are
registered as green polygons. The cyan dots represent topological waypoints.
The Directed Visibility Graph is marked as cyan and yellow lines. (a)
Overview. (b-c) The robot can manipulate the movable obstacles during the
navigation and can switch the contact points during manipulation. (d) The
robot attempts to move a heavy object ①. When the robot contacts the object,
the cost of moving it is updated from force feedback. If the object is
affordable, the robot pushes it, otherwise it replans the alternate strategy
to execute. The robot can quickly adapt to new sensory observations no matter
whether a movable object is affordable or not.

Interactive navigation is a navigation task that involves interactions between
the robot and movable obstacles. Environmental interaction is essential
especially for robots navigating cluttered spaces. This paper proposes a
solution for interactive navigation in cluttered unknown environments,
focusing on fast and adaptable navigation with environmental interactions. The
problem remains challenging as it needs to consider multiple sub-tasks
simultaneously, which include: 1) Dynamically updating the environment
representation with new sensor data. 2) Executing a global path planning to
avoid local minima, which considers environmental interactions during the path
search. 3) Strategizing interaction planning to manipulate objects. In cases
where the environment is complex with numerous movable obstacles, the
problem’s computational complexity increases significantly. Consequently,
ensuring the algorithm operates in real time becomes a challenge.

Our work proposes a real-time framework that consists of identification /
mapping, path planning, and interaction. This is achieved through two key
ideas.

The first idea is a hybrid map representation called Directed Visibility Graph
(DV-graph). This graph’s edges represent roads and the traversability between
the connected vertices. Some of the edges also encode strategies on how to
manipulate movable obstacles away from the path. The sparse nature of DV-Graph
and its integrated interaction strategies allow for a global path search with
low latency (within 10ms on a 4.7GHz i7) while also considering environmental
interactions. DV-graph is updated hierarchically at each data frame. It
consists of two layers. The local layer converts the sensor data into a
polygon map, on which interaction planning for each movable obstacle is
performed. The prospective strategies are encoded into visibility edges and
connected between polygons to form the DV-graph. The global layer is a larger
DV-graph that is merged and updated with the local layer at each frame. The
hierarchical and incremental graph construction framework ensures real-time
performance during navigation. This framework also supports manipulation
actions such as push, pull, and pick-up. These can be encoded into the same
map representation as a multi-level DV-graph for rapid global path search.

The second key idea is adaptable interaction planning via kinodynamic path
search. It makes the robot actively attempt to move obstacles, update their
affordances, and adapt strategies accordingly. Given a local region divided
into multiple components by a movable obstacle with unknown physics
properties, as shown in Fig. 3, the planner tries to generate a series of
kinodynamically feasible manipulation strategies that reconnect sub-regions
based on assumed physical properties. These strategies enable the robot to
clear the object from its path. During the navigation, the robot attempts to
move the object by executing the above strategy, the planner updates the
physics properties, thus updating the affordance, of the object through
sensory feedback. The robot continues executing the original strategy if the
estimated affordance is within a certain range. Otherwise, the whole global
path is replanned. The updated affordance is then stored in the DV graph for
later local and global planning.

The proposed algorithm is validated in complex environments of different
scales and with movable obstacles. Extensive experiments show that our
algorithm is faster than most benchmarks in the order of magnitudes while
maintaining up to $49\%$ higher path efficiency.

The main contributions can be summarized as:

  * •

An improved dynamic DV-graph that encodes the interaction strategies and
accelerates path finding.

  * •

An interaction planning method that adapts to the online physics property
evaluation of movable obstacles.

  * •

A complete navigation algorithm for interactive navigation in unknown or
partially known environments.

We open-source our code in the docker demo to facilitate future research.

##  II Related Work

####  II-1 Navigation in Unknown Environments

Navigation in unknown environments has been studied and applied in many field
applications, such as for exploration [1, 2], inspection [3], and swarm
formation [4]. However, these works regard all obstacles as static and fixed,
which limits the robot’s ability to move in cluttered environments.

####  II-2 Interactive Navigation - Geometry Based

While works on navigation are prolific, there is less attention on utilizing
manipulation for better navigation. Previous studies on this problem formulate
this problem as a geometric motion planning problem called navigation among
movable objects (NAMO). Wilflong[5] proved that NAMO is NP-hard. Stilman et
al.[6] derived optimal solutions for 2D geometric planning for a subset of
NAMO problems called $LP1$ with complete knowledge of the environment and
objects. Levihn et al. [7] proposed a locally optimal algorithm for $LP1$ NAMO
problems in partially known environments, and Muguira-Iturral et al. [8] takes
visibility into account in NAMO problem. However, solving a local NAMO problem
that is moderately difficult takes a significant amount of time. In [6], it
takes seconds due to the large search space of manipulations in a long horizon
on a dense grid map. In addition, the manipulation actions are restricted to
fixed contact points or directions, and the observation of object shape is
assumed to be perfect. These disadvantages make them infeasible in complex
environments and hinge their potential for real-world deployment.

####  II-3 Interactive Navigation - Learning Based

There’s a recent trend for mapping visual input to agent’s actions using
reinforcement learning. Xia et al. [9] compared different reinforcement
learning algorithms to solve interactive navigation problems, where the robot
can push the objects to shorten the path to the goal. However, the agent
relies on a precomputed shortest path to operate, which is hard to achieve in
real world settings. An end-to-end visual interaction navigation framework is
proposed in [10]. However, the interaction is simplified to moving a single
object with a few predefined actions to reach a goal within its sensor range.

This work focuses on developing a real-time adaptable interactive navigation
algorithm in complex environments, where all the approaches above fail. The
introduced DV-graph based mapping and path searching algorithm facilitates
rapid path planning in complex environments. The adaptable interaction
planning algorithm empowers the robot to attempt obstacle manipulation, infer
the manipulation affordance via tactile sensing, update the navigation graph,
and adapt its strategies according to the latest navigation graph.

##  III Problem Definition

Figure 2:  A diagram of our system architecture.

Our problem is divided into two sub-problems: interactive motion planning and
adaptive manipulation.

For the interactive motion planning problem, define
$\textit{Q}\subset\mathbb{R}^{3}$ as the work space for the robot to navigate.
Let $\mathcal{S}\subset\textit{Q}$ be the perceived sensor data. Here,
$\mathcal{S}$ is classified as the object-level point clouds, which contains
the points of movable objects $\mathcal{S}_{mov}\subset\mathcal{S}$ and points
of the background obstacles
$\mathcal{S}_{bg}=\mathcal{S}\backslash\mathcal{S}_{mov}$. We proposed a
directed visibility graph (DV-graph) denoted as
$\mathcal{G}=(\textit{V},\Pi)$, from $\mathcal{S}$, where
$\mathbf{v_{i}}\in\textit{V}$ represents one vertex and
$\langle\mathbf{v_{p},v_{q}}\rangle\in\Pi$ means an ordered vertex pair in
$\mathcal{G}$. The problem can be expressed as:

###### Problem 1

Given the robot position $\mathbf{p}_{robot}\in\textit{Q}$ and goal point
$\mathbf{p}_{goal}\in\textit{Q}$, find the most energy-efficient path between
$\mathbf{p}_{robot}$ and $\mathbf{p}_{goal}$ on $\mathcal{G}$. The energy
efficiency considers both the travel distance and effort efficiency when
manipulating objects to move them out of the way.

Problem 1 is solved repetitively in each planning cycle. During navigation, we
online update $\mathcal{G}$ with new sensor data, mapping the newly perceived
environment and updating the maintained global map. Then, we re-plan the path
on the updated $\mathcal{G}$ in the next planning cycle until arriving at
$\mathbf{p}_{goal}$.

For adaptive manipulation, define $\textit{Q}_{local}\subset\textit{Q}$ as the
local workspace for a single manipulation. Let
$\\{\mathcal{C}^{i}_{local}\subset\mathcal{Q}_{local}|i\in\mathbb{Z}^{+}\\}$
be locally disconnected components that are divided by a movable object, as
shown in Fig. 3. The manipulation strategies are defined as motion primitives
$\pi$. The problem is described as:

###### Problem 2

Given $\textit{Q}_{local}$ that is divided into $n$ components
$\\{\mathcal{C}_{local}\\}$ by a movable object, find $n(n-1)$ manipulation
primitives $\pi$ that re-connect any two components.

Problem 2 is solved once for each object that blocks a local space. The
manipulation primitives are stored in the map with the object and utilized in
two ways. First, it helps the DV-graph determine the travel cost from one
component to another to trade off between manipulating or taking a collision-
free path. Second, the searched primitives guide the robot’s actual
manipulation motion controller.

##  IV System Overview

The architecture of the proposed algorithm is shown in Fig. 2. The algorithm
comprises two primary components: DV-graph construction and updating (Sects. V
and VI), and motion planning (Sect. VII). In the DV-graph construction and
update phase, the process begins with pre-processed sensor data
$\mathcal{S}_{mov}$ and $\mathcal{S}_{bg}$. Then, a polygon extraction
procedure transforms $\mathcal{S}_{mov}$ and $\mathcal{S}_{bg}$ into two sets
of polygons. These sets form the map representation for the DV-graph.

The topological space connectivity analysis algorithm is designed to determine
whether a single movable object divides a local space into several
disconnected components. The interaction planning module is employed for each
movable object. This module generates manipulation strategies to reconnect
these locally disconnected spaces and calculates the associated manipulation
costs. The output of the simulation module is a comprehensive set of
manipulation strategies, encompassing all feasible scenarios considering both
the starting and ending states. Finally, the local DV-graph is constructed
with the extracted polygons and the manipulation strategies in order to encode
both the geometry and interaction information. The local DV-graph is merged
with the global V-graph in each running cycle, facilitating its dynamic
update.

In the planning phase, the process initially searches a global path from
$\mathbf{p}_{robot}$ to $\mathbf{p}_{goal}$. During the path execution , the
choice of local planner depends on the robot’s its current navigation context:
a collision-free planner is employed when the robot navigates between two
visible waypoints. An interactive planner is utilized for mobile manipulation
if the current local path segment intersects with movable objects, an
interactive planner is utilized for mobile manipulation. During the
manipulation, affordance of the object are dynamically updated. The
interactive planner then adaptively revises the manipulation policy based on
these updated parameters. Subsequently, these revised parameters are
incorporated to update the corresponding edges in the global DV-graph.

##  V DV-graph Construction and Update

###  V-A Polygon Extraction

To differentiate between static and movable objects in the DV-graph, this
module takes in $\mathcal{S}_{mov}$ and $\mathcal{S}_{bg}$ separately and
transform them into two sets of polygons, denoted as
$\\{\mathcal{P}^{i}_{mov}\subset\mathcal{Q}|i\in\mathbb{Z}^{+}\\}$ and
$\\{\mathcal{P}^{i}_{bg}\subset\mathcal{Q}|i\in\mathbb{Z}^{+}\\}$. To achieve
that, we first inflate $\mathcal{S}_{mov}$ and $\mathcal{S}_{bg}$ based on the
robot dimension, and register them to 2-D image planes. Utilizing the methods
in [11] and [12], enclosed polygons are extracted and simplified. This polygon
extraction step is developed from [13], detailed explanation is available
therein. Finally, the two sets of polygons are combined into a complete local
polygon graph, denoted as
$\\{\mathcal{P}^{i}_{local}\subset\mathcal{Q}|i\in\mathbb{Z}^{+}\\}$, as shown
in Fig. 1(a). The $\mathcal{P}_{bg}$ are marked as red polygons and
$\mathcal{P}_{mov}$ are marked as green. Notably, polygons from different
classes can intersect, but no intersections occur within the same class due to
the topological structure analysis based on [11].

Figure 3:  (a-b) Path-disconnected components $\\{\mathcal{C}^{i}_{local}\\}$
of a local region $\\{\mathcal{P}^{i}_{local}\\}$. Green polygon represents
$\mathcal{P}_{mov}$ and grey ones are $\mathcal{P}_{bg}$. They are inflated
based on the robot dimension. Topological waypoints $\\{\mathbf{p}_{topo}\\}$
are marked as purple, potential manipulation strategies are represented as
directed edges in purple. (c) Demonstration of the process to choose
$\\{\mathbf{p}^{i}_{topo}\\}$. Intersected points $\mathbf{p}_{inter}$ are
marked in cyan. (d) Illustration of the interaction planning module. Contact
point are marked as blue.

###  V-B Topological Space Connectivity Analysis

The free-space of the static background polygon map, denote as
$\mathcal{Q}\backslash\\{\mathcal{P}^{i}_{bg}\\}$, is guaranteed to be a path-
connected space [14] because of the topological structure analysis. This
implies that for any point pairs
$\langle\mathbf{p}_{i},\mathbf{p}_{j}\rangle\in\mathcal{Q}\backslash\\{\mathcal{P}^{i}_{bg}\\}$,
there always exist a connected path. However, when
$\\{\mathcal{P}^{i}_{bg}\\}$ and $\\{\mathcal{P}^{i}_{mov}\\}$ are combined,
certain regions become locally path-disconnected, as shown in Fig. 3. The
space is divided into several locally disconnected components,
$\\{\mathcal{C}^{i}_{local}\subset\mathcal{Q}|i\in\mathbb{Z}^{+}\\}$. To
navigate through these disconnected spaces, the robot needs to actively
manipulate the movable object to re-establish connectivity between regions. As
illustrated in Fig. 3(a)-(b), the number of disconnected components leads to
$n(n-1)$ potential manipulation strategies based on the robot’s start and goal
regions.

As shown in Fig. 3(c), For each $\mathcal{C}^{i}_{local}$, define
$\mathbf{p}^{i}_{topo}\in\mathcal{C}^{i}_{local}$ as the representative point
of $\mathcal{C}^{i}_{local}$, which is called topological waypoints because
different $\mathbf{p}^{i}_{local}$ belongs to different topological spaces. To
make topological waypoints have good visibility with other vertices in
$\mathcal{C}^{i}_{local}$, the following strategy is adopted. Given a pair of
consecutive intersection points
$\langle\mathbf{p}_{inter}^{i},\mathbf{p}_{inter}^{i+1}\rangle$, marked as
cyan in Fig. 3(c), calculate its mid-point $\mathbf{p}_{mid}$ and normal
vector
$\vec{\mathbf{n}}=\overrightarrow{\mathbf{p}_{inter}^{i}\mathbf{p}_{inter}^{i+1}}\times(0,0,1)$.
Then create its normal vector starts from $\mathbf{p}_{mid}$, and calculate
its intersection point with the polygon, denote as $\mathbf{p}^{\prime}$ and
mark as red point in the figure. Finally, $\mathbf{p}^{i}_{topo}$ can be
calculated as:

$\mathbf{p}^{i}_{topo}=\mathbf{p}^{\prime}+\vec{\mathbf{n}}\cdot\min(d_{thres},\frac{d_{obs}}{2}),$
(1)

where $d_{obs}$ represents the minimal distance from $\mathbf{p}^{\prime}$ to
its closest obstacle along $\vec{\mathbf{n}}$, $d_{thres}$ is a pre-defined
threshold value with unit meters, here we choose $0.5$ as the $d_{thres}$.

###### Theorem 1

$\mathbf{p}^{i}_{topo}$ has at least 3 visible vertices in
$\mathcal{C}^{i}_{local}$.

Proof: To prove it, two axioms are introduced: i) Any polygon can be
decomposed into triangles. ii) Any point in a triangle is visible to all three
vertices. Given $\mathcal{C}^{i}_{local}$, it can be decomposed to a set of
triangles $\\{\Delta_{k}\subset\mathcal{C}^{i}_{local}|k\in\mathbb{Z}^{+}\\}$,
because $\mathbf{p}^{i}_{topo}$ is within $\mathcal{C}^{i}_{local}$, it must
be in one of $\\{\Delta_{k}\\}$, thus connecting at least three vertices in
$\mathcal{C}^{i}_{local}$.

If the robot can manipulate the object from $\mathcal{C}^{i}_{local}$, and
achieves $\mathcal{C}^{j}_{local}$, then the point pair
$\langle\mathbf{p}^{i}_{topo},\mathbf{p}^{j}_{topo}\rangle$ is connected added
into the DV-graph as a travelsable edge. The travelsibility of the edge is
assigned based on the estimated cost of the manipulation.

Input : Segmented Sensor Data: $\mathcal{S}_{bg},\mathcal{S}_{mov}$, DV-graph:
$\mathcal{G}$

Output : Updated DV-graph: $\mathcal{G}$

1 $\\{\mathcal{P}_{bg},\mathcal{P}_{mov}\\}\leftarrow$
PolygonExtraction($\mathcal{S}_{bg},\mathcal{S}_{mov}$);

2

3 $\triangleright$ Local DV-graph update

4Construct local DV-graph on $\\{\mathcal{P}^{i}_{bg}\\}$;

5 for  _each $\\{\mathcal{P}_{mov}\\}$_ do

6 for  _each pair $\langle\mathbf{p}_{topo}^{i},\mathbf{p}_{topo}^{j}\rangle$_
do

7 $\mathcal{J},\pi\leftarrow$
$\Gamma$($\langle\mathbf{p}_{topo}^{i},\mathbf{p}_{topo}^{j}\rangle$, $\mu$)

8 end for

9

10 end for

11

12 $\triangleright$ Local-global DV-graph merge

13for  _each vertex
$\mathbf{v}_{p}\in\\{\mathcal{P}^{i}_{local}\\}\cup\\{\mathcal{P}^{j}_{global}\\}$_
do

14 if  _an association exists_ then

15 Update the vertex in $\\{\mathcal{P}^{j}_{global}\\}$;

16

17 else if  _$\mathbf{v}_{p}$ only exists in $\\{\mathcal{P}^{j}_{local}\\}$ _
then

18 Add $\mathbf{v}_{p}$ to $\\{\mathcal{P}^{j}_{global}\\}$ as a new vertex;

19

20 else

21 Remove $\mathbf{v}_{p}$ from $\\{\mathcal{P}^{j}_{global}\\}$ based on
voting;

22

23 end if

24

25 end for

26

27return Merged DV-graph $\mathcal{G}$;

28

Algorithm 1 DV-graph Update

###  V-C Interaction Planning

Given a locally path-disconnected region with n-components, as shown in Fig.
3(a)-(b), the module is designed to generate $n(n-1)$ local manipulation
policies that make each
$\langle\mathbf{p}^{i}_{topo},\mathbf{p}^{j}_{topo}\rangle$ re-connected. The
input of this module is the local contour graph $\\{\mathcal{P}_{local}\\}$.
For each movable object
$\\{\mathcal{P}^{i}_{mov}\subset\mathcal{P}_{local}\\}$, the simulation
process is described in Algorithm 1, Line 5-9. Given an arbitrary pair of
topological waypoints
$\langle\mathbf{p}^{i}_{topo},\mathbf{p}^{j}_{topo}\rangle$ with the estimated
push affordance $\mu$ of the object, the strategy generation function
$\Gamma:\mathbb{R}^{2}\times\mathbb{R}^{2}\rightarrow\mathbb{R}\times\mathbf{SE(2)}$
outputs the interaction strategies $\pi$ along with the estimated manipulation
cost $\mathcal{J}$. Because the output does not restrict manipulation actions,
general real-time mobile manipulation solutions can be conveniently adopted
into this framework with minor changes. The detailed design of the interaction
strategies generation is discussed in Section VI-B.

###  V-D Local-Global DV-graph Update

After constructing $\\{\mathcal{P}_{local}\\}$, we merge the overlapped area
of $\\{\mathcal{P}_{local}\\}$ and $\\{\mathcal{P}_{global}\\}$ to update the
global DV-graph $\mathcal{G}$. The process is described in Algorithm 1, Line
11-19. For each vertex in $\\{\mathcal{P}_{local}\\}$, we check if there’s a
close match in $\\{\mathcal{P}_{global}\\}$ within a certain distance
threshold. If so, we update the existing vertex in
$\\{\mathcal{P}_{global}\\}$. If the vertex only exists in
$\\{\mathcal{P}_{local}\\}$, we regard it as a new vertex and add it into the
$\\{\mathcal{P}_{global}\\}$. Conversely, vertices only in
$\\{\mathcal{P}_{global}\\}$, which means they are not observed in the current
frame, are not immediately removed. To mitigate sensor noise, we develop a
voting mechanism where a vertex is removed from $\\{\mathcal{P}_{global}\\}$
only after being continuously unobserved for several frames.

##  VI Interaction Strategy Generation

In this section, we propose our solution for Problem 2. We first decompose the
Problem 2 into a series of sub-problems (Problem 3). For each sub-problem, we
conduct a kinodynamic path search for a feasible interaction strategy by
expanding motion primitives with a discretized control input space (Sect.
VI-B). Our novel design of heuristics is proven to be admissible in general
cases and fits well with our sparse polygon-based map representation.

###  VI-A Problem Decomposition

As discussed in Sect. V-C, there are $n(n-1)$ local policies to be generated.
It is hard to solve via a single search because the state space is large and
with multiple sub-goals, and it is hard to design heuristics with an
admissibility guarantee. Therefore, we decompose the original problem into
$n(n-1)$ sub-problems and solve them separately.

The process of the problem decomposition is illustrated in Fig. 4. Each sub-
problem is defined as:

###### Problem 3

Given the start position $\mathbf{p}^{i}_{topo}$ and goal position
$\mathbf{p}^{j}_{topo}$, find the most energy-efficient manipulation policy
that connects $\mathbf{p}^{i}_{topo}$ and $\mathbf{p}^{j}_{topo}$ via a local
collision-free path.

The problem is solved by a hybrid A* path search method with a novel heuristic
design. It has two advantages: 1) the found interaction strategy, or path, is
kinodynamically feasible for the robot to execute, and 2) the designed
heuristics are admissible in general cases, which improved the optimality of
the path. Technical details can be found in Sect. VI-C.

Figure 4:  Illustration of the problem decomposition. (a) The main problem
contains $n(n-1)$ sub-problems. (b-d) Three typical sub-problems. The upper-
right corner of (b) explains the calculation of the heuristics. The left-upper
corner of (d) demonstrates the case when there are multiple heuristic
polygons.

###  VI-B State Transition Model

In this section, we discuss the state transition modeling in the kinodynamic
path search algorithm. For simplicity of our system, we use differential
vehicles: $v=[v_{x},0],\omega\in\mathbb{R}$, and use push manipulation as our
interacting method.

Due to the sensor limitation of mobile robots, our system only infer the
object’s shape from on-board depth input (where occlusion is inevitable) and
physics properties by push attempts. For this reason, we choose stable
push[15] as our scheme. It’s more robust to the estimation error of object’s
shape and physics properties compared with other push schemes[16, 17, 18]. It
also simplifies the generation of kinodynamically feasible push strategies for
our vehicle.

Given an object with known shape, friction center, contact edge, and the robot
to object friction coefficient, we can identify a set of potential stable
rotation centers $P_{s}$ by the procedure ”STABLE” in [15]. In our case, the
object shapes and physics properties are unknown at first. We estimate the
shape of the object in V-A, and start with an initial robot to object friction
coefficient $k$. It is assumed to be uniform so that the friction center
aligns with its geometric center. Then, for a certain contact edge (which
determines the robot state), we select the vehicle’s control inputs which will
only result in stable pushes of the object, and map them to the state of
objects. Such mapping is trivial because of stable push.

For our case, the full state space $\mathbb{S^{\prime}}$ of problem 2 is the
combination of the robot’s and object’s states:
$\mathbb{S^{\prime}}\subset\mathbf{SE(2)}\times\mathbf{SE(2)}$. Because of the
bijection between the robot state and the object’s contact points, the robot’s
state can be simplified as a set of available contact points
$\mathbf{C}\subset\mathbb{R}^{2}$, shown as blue points in Fig. 3(d). The
state space is then simplified to
$\mathbb{S}\subset\mathbf{C}\times\mathbf{SE(2)}$. The state vector of a
movable object is comprised of object position and orientation
$x=[p_{x},p_{y},\psi]^{T}$ and available contact points
$c=[c^{1},c^{2},...,c^{n}]^{T}$, where $c^{i}\in\mathbf{C}$. The state
transition model is then derived by: a) applying the differential vehicle
model to the movable object and aligning the heading with the normal of the
stable push edge. b) restrictig $\frac{\omega}{v_{x}}$ to let the
instantaneous rotation center $p$ fall into $P_{s}$. A state transition model
whose actions are feasible and transitions are robust is generated. The
physics properties will update during the execution of push strategies, which
we discuss in VII.B.

Figure 5:  Hybrid A* push search with contact point switch.

###  VI-C Hybrid A-Star Path Search

Given a pair of topological waypoints, the problem 3 is solved in two steps.
First, find the polygon with the smallest area that hinders the connection of
these topological waypoints, demonstrated as blue polygons in Fig. 4(b-d).
Here we call it heuristic polygon because it provides heuristics to guide the
path search. We represent it as
$\\{\mathcal{P}_{heu}\\}=\\{\mathcal{P}^{i}_{mov}\\}\cap\\{\mathcal{P}_{bg}\\}$.
If there are multiple heuristic polygons between the two regions,
$\\{\mathcal{P}_{heu}\\}$ is chosen as the union of the vertex set of all
these polygons, as shown in Fig. 4(d). The heuristic (or the negative reward)
of the path search is the minimum distance between parallel lines of the
polygon, which is mathematically defined to be the width of the polygon[19]. A
hybrid-state astar is then performed with the state and transition model
discussed in Sect. VI-B.

###  VI-D Completeness and Admissibility

The completeness cannot be theoretically guaranteed because the state space is
discredited. However, our method supports variable-resolution state space to
improve the completeness guarantee [20] and the practical performance is
satisfactory. For optimality, we can prove the admissibility when the
heuristic polygon $\\{\mathcal{P}^{i}_{heu}\\}$ or the union of all vertices
of multiple heuristic polygons $\\{\mathcal{P}^{union}_{heu}\\}$ are convex.

###### Lemma 1

$h$ is admissible if $\\{\mathcal{P}^{i}_{heu}\\}$ is convex.

According to the definition stated in [19], $h$ is the minimum distance that
disengage two convex polygons from contact. Therefore, $h$ is admissible.

###### Theorem 2

$h$ is admissible if $\\{\mathcal{P}^{union}_{heu}\\}$ is convex.

Proof: As shown in the upper-left part of Fig. 4(d), if the union is convex,
$h$ must satisfy the condition that $h\leq h_{1}+h_{2}$. Because $h_{1}$ and
$h_{2}$ are both admissible for their heuristic polygons, therefore $h$ is
admissible for the union.

##  VII Interactive Motion Planning and Online Adaptation

###  VII-A Global Path Search on DV-graph

To identify the DV-graph $\mathcal{G}$ for the shortest path from
$\mathbf{p}_{robot}$ to $\mathbf{p}_{goal}$, the planner first add
$\mathbf{p}_{robot}$ and $\mathbf{p}_{goal}$ as two vertices, connecting them
to other visible vertices in ${P^{i}_{global}}$. A breadth-first search is
then executed on $\mathcal{G}$ to find the shortest path, denoted as a set of
waypoints
$\mathbf{P}_{path}=\\{\mathbf{p}^{i}_{wp}\in\mathcal{Q}|i\in\mathbb{Z}^{+}\\}$,
if it exists. During the navigation among the unknown environment, vertices
and directed visibility edges are dynamically updated and stored in
$\mathcal{G}$. In the subsequent operations, the DV-graph $\mathcal{G}$ can be
loaded as a prior map. When the environment is fully explored, the global
planner is capable of efficiently conducting real-time, globally optimal path
searches in complex environments ($455m$ path with $1570$ vertices in $3ms$,
as tested).

###  VII-B Path Execution and Adaptable Replan

During the navigation, the robot switches the local planner based on the
current path segment it is executing. The working principle is demonstrated in
Algorithm 2. The robot employs collision-free planner [21] for regular
navigation. Upon nearing a topological waypoint, it switches to interactive
planner to execute the planned interaction strategies discussed in Sect. VI.
The interactive planner considers the following constraints during the
runtime: push stability, collision avoidance and push affordance $\mu$ of the
robot (e.g., how much weight can it push, the friction coefficient between
pusher and slider). We utilize the same method discussed in Sect. VI-B for
path replan.

With each sensor frame, the robot utilizes tactile sensing to update the
physical properties of the movable obstacle and therefore update the push
affordance. The design of our tactile feedback module is based on three
principles:

  1. 1.

If the state of the object doesn’t change after maximum push effort, we mark
the object as not pushable.

  2. 2.

If $||\mathcal{J}-\hat{\mathcal{J}}||$ exceeds a threshold, update
$\mathcal{J}$ and $u_{x}$.

Based on the updated $\mathcal{J}$ and $u_{x}$, the planner adaptively re-
plans the interaction primitive or the global path as necessary. After the
push execution, the updated $\mathcal{J}$ and $\pi$ are encoded into the DV-
graph $\mathcal{G}$. No matter whether the object is affordable, the proposed
method can always adapt to the affordance update and replan the proper
strategy accordingly.

Input : Next waypoint: $\mathbf{p}_{path}^{next}\in\mathbf{P}_{path}$,

start: $\mathbf{p}_{robot}$, goal: $\mathbf{p}_{goal}$, cost: $\mathcal{J}$,

affordance: $\mu$

1

2while  _$\mathbf{p}_{robot}\neq\mathbf{p}_{goal}$_ do

3 if  _$\mathbf{p}_{wp}^{next}\in{\mathbf{p}_{topo}}$ and
$||\mathbf{p}_{wp}^{next}-\mathbf{p}_{robot}||<r$ _ then

4
$\hat{\mathcal{J}},\hat{\pi},\hat{\mu}\leftarrow\Gamma(\langle\mathbf{p}_{robot},\mathbf{p}_{wp}^{next}\rangle,\mu)$;

5 if  _ $||\hat{\mathcal{J}}-\mathcal{J}||>\tau_{thres}$ _ then

6 $\mathcal{J}\leftarrow\hat{\mathcal{J}}$; $\pi\leftarrow\hat{\pi}$;
$\mu\leftarrow\hat{\mu}$;

7 Update correlated edges in $\mathcal{G}$ based on $\hat{\mathcal{J}}$;

8 Replan the global path;

9 else

10

11 InteractivePlanner($\pi$);

12 end if

13

14 else

15 CollisionFreePlanner($\mathbf{p}_{wp}^{next}$);

16

17 end while

18

Algorithm 2 Path Execution and Adaptation

##  VIII Experiments

To demonstrate the effectiveness of our interactive navigation algorithm, we
conduct $10$ navigation tasks in simulated environments with different scales,
as shown in Fig. 6. For each task, The start and goal position are chosen
based on the criteria that the optimal path length is larger than a threshold.
The threshold is set as $15m$ for the $32m\times 32m$ environment, and $80m$
for the $330m\times 270m$ one. The simulated ground vehicle is equipped with a
Velodyne Puck Lidar used as the range sensor for perception, a 1-D force
sensor for tactile sensing. The framework runs on a laptop with i7-12700H CPU.
We configure the algorithm to update the DV-graph at 2.5Hz and perform path
search at each graph update. The spatial resolution is set as $0.15m$. The
local layer is a $60\times 60m$ area with the vehicle in the center.

Figure 6:  Environments overview.

###  VIII-A Computational Efficiency Comparison

To demonstrate the computational efficiency of the proposed framework, we
compare our algorithm with three classical path planning methods: A*, RRT* and
BIT*, two NAMO methods: R-NAMO and LAMB, and our previous work FAR that uses a
DV-graph without considering interaction. Here, A* is a representative search-
based method, BIT* is considered as the state-of-the-art in sampling-based
method, R-NAMO and LAMB are two representative methods for NAMO in known and
unknown environments, and FAR is considered as the latest global path planning
method in unknown environments. The experiment results are demonstrated in
Table. I. The italic font indicates that the algorithm is implemented in
Python, otherwise implemented in C++. The ground truth (GT) optimal path is
noted as the length of the path search by A* on the dense grid map without
inflation. The average time consumption is recorded in Table. I if the path
search is successful. The time recorded for sampling-based methods represents
the duration until the method initially finds a sub-optimal path, the length
of which is no more than 1.5 times the length of the GT optimal path.

As Table. I shows, considering that the speed of Python can be up to $96.3$
times slower than C++ [22], all methods have impressive performance in the
room environment. In the office scenario, search-based method runs slower due
to the increase of the environment scale, while all NAMO methods fail in this
scale. In the tunnel scene, Only FAR and our method can run in real-time and
maintain similar speeds. The reasons for the performance of our work and FAR
can be attributed to 1) the sparse nature of the map representation makes the
path search faster in orders of magnitude, and 2) the efficiency of C++ makes
the algorithm runs faster.

###  VIII-B Path Efficiency Comparison

To demonstrate the path efficiency of the proposed system, we conducted this
experiment using the same experiment settings as Sect. VIII-A. Table. II
presents the results of the experiments. The performance of path efficiency is
evaluated by SPL[23]. As shown in the table, A* performs best in the room
scenario because of its mapping density and optimality guarantee. In the room
with movable obstacles, R-NAMO and LAMB have lower SPL because their
manipulation policies take redundant actions. However, our method can plan
high quality manipulation policies because it is admissible in general cases.
In Office and Tunnel, with the increasing scale of the environments, the path
length of manipulation actions has a lower ratio than the total path length;
therefore, our method achieves higher SPL in these scenarios. However, R-NAMO
and LAMB fail due to their low scalability, and other methods have to take
alternate paths or even fail in some sub-tasks as they cannot conduct
interactive navigation, thus resulting in lower SPL.

TABLE I: Average Search Time in [ms] Environment |  | Room
---
(no objects)
| Room
---
(with objects)
Office | Tunnel
| Optimal
---
Path Length (m)
8.6 | 8.6 | 47 | 455
A* | 2.2/179.0(py) | - | 16.0 | 363.5
RRT* | 0.4 | - | 0.4 | 325.2
BIT* | 0.17 | - | 0.7 | 1.1e3
R-NAMO | 1.1e3(py) | 1.2e3(py) | - | -
LAMB | 7.0e3(py) | 34.8e3(py) | - | -
FAR | 0.3 | - | 0.4 | 1.27
Ours | 0.3 | 0.3 | 0.4 | 1.32
TABLE II: SPL - Success weighted by Path Length Environment |  | Room
---
(no object)
| Room
---
(with object)
Office | Tunnel
A* | 0.97 | - | 0.41 | 0.81
RRT* | 0.84 | - | 0.37 | 0.77
BIT* | 0.83 | - | 0.39 | 0.80
R-NAMO | 0.96 | 0.58 | - | -
LAMB | 0.96 | 0.61 | - | -
FAR | 0.96 | - | 0.43 | 0.79
Ours | 0.96 | 0.70 | 0.86 | 0.97

###  VIII-C System-level Comparison

To illustrate the potentiality and reliability of our algorithm in field
application scenarios, we compare the systematic navigation performance with
FAR. The reasons for comparing these two algorithms are: 1) the superiority of
FAR over other path search methods has been demonstrated in [13], and 2) the
offline implementation of R-NAMO and LAMB make them fail to be applicable
navigation systems for comparison. We choose Office and Tunnel for test
because they are more close to the field applications. The evaluation metrics
included travel distance, navigation time, and success rate for various sub-
tasks, with both systems configured identically in terms of local map size,
resolution, and update frequency.

The results are shown in Fig. 8-8 and Table. III. Both algorithm achieves
satisfying performance in terms of success rate. However, due to the lack of
interactive navigation ability, FAR planner makes more attempts to finish each
sub-task, and sometimes even fail to achieve the goal, e.g. waypoint D in Fig.
6(b). The results in Table. III proves that our algorithm has higher
efficiency in different environments.

TABLE III: System-level Comparison

Environment | Office | Tunnel
---|---|---
Metrics | Travel Distance (m) | Time (s) | Success Rate | Travel Distance (m) | Time (s) | Success Rate
FAR | 232.56 | 360.34 | 4/5 | 1489.63 | 1911.45 | 3/3
InteractiveFAR | 109.72 | 242.89 | 5/5 | 1120.91 | 1486.78 | 3/3

Figure 7: The resulting map and trajectories of system-level experiment in
Office.

Figure 8: The resulting map and trajectories of system-level experiment in
Tunnel.

##  IX Conclusion

In this paper, we present InteractiveFAR, an interactive navigation system for
complex unknown environments cluttered with movable obstacles. Utilizing
dynamic DV-graph that integrates interactions during mapping, the system
outperforms benchmarks in path searching. The proposed interactive motion
planning and adaptive replan framework helps our system manipulate movable
obstacles and adjust strategies in real-time based on new sensor data.
Comprehensive experiments and benchmark comparisons validate the efficiency
and potential of our system in field applications.

## References

  * [1] C. Cao, H. Zhu, H. Choset, and J. Zhang, “Tare: A hierarchical framework for efficiently exploring complex 3d environments.” in _Robotics: Science and Systems_ , vol. 5, 2021.
  * [2] A.-H. Shahidzadeh, S. J. Yoo, P. Mantripragada, C. D. Singh, C. Fermüller, and Y. Aloimonos, “Actexplore: Active tactile exploration on unknown objects,” _arXiv preprint arXiv:2310.08745_ , 2023.
  * [3] F. Farahnakian, L. Koivunen, T. Mäkilä, and J. Heikkonen, “Towards autonomous industrial warehouse inspection,” in _2021 26th International Conference on Automation and Computing (ICAC)_. IEEE, 2021, pp. 1–6.
  * [4] X. Zhou, X. Wen, Z. Wang, Y. Gao, H. Li, Q. Wang, T. Yang, H. Lu, Y. Cao, C. Xu _et al._ , “Swarm of micro flying robots in the wild,” _Science Robotics_ , vol. 7, no. 66, p. eabm5954, 2022.
  * [5] G. Wilfong, “Motion planning in the presence of movable obstacles,” in _Proceedings of the fourth annual symposium on Computational geometry_ , 1988, pp. 279–288.
  * [6] M. Stilman and J. Kuffner, “Planning among movable obstacles with artificial constraints,” _The International Journal of Robotics Research_ , vol. 27, no. 11-12, pp. 1295–1307, 2008.
  * [7] M. Levihn, M. Stilman, and H. Christensen, “Locally optimal navigation among movable obstacles in unknown environments,” in _2014 IEEE-RAS International Conference on Humanoid Robots_. IEEE, 2014, pp. 86–91.
  * [8] J. Muguira-Iturralde, A. Curtis, Y. Du, L. P. Kaelbling, and T. Lozano-Pérez, “Visibility-aware navigation among movable obstacles,” in _2023 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2023, pp. 10 083–10 089.
  * [9] F. Xia, W. B. Shen, C. Li, P. Kasimbeg, M. E. Tchapmi, A. Toshev, R. Martín-Martín, and S. Savarese, “Interactive gibson benchmark: A benchmark for interactive navigation in cluttered environments,” _IEEE Robotics and Automation Letters_ , vol. 5, no. 2, pp. 713–720, 2020.
  * [10] K.-H. Zeng, L. Weihs, A. Farhadi, and R. Mottaghi, “Pushing it out of the way: Interactive visual navigation,” in _Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition_ , 2021, pp. 9868–9877.
  * [11] S. Suzuki _et al._ , “Topological structural analysis of digitized binary images by border following,” _Computer vision, graphics, and image processing_ , vol. 30, no. 1, pp. 32–46, 1985.
  * [12] D. H. Douglas and T. K. Peucker, “Algorithms for the reduction of the number of points required to represent a digitized line or its caricature,” _Cartographica: the international journal for geographic information and geovisualization_ , vol. 10, no. 2, pp. 112–122, 1973.
  * [13] F. Yang, C. Cao, H. Zhu, J. Oh, and J. Zhang, “Far planner: Fast, attemptable route planner using dynamic visibility update,” in _2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2022, pp. 9–16.
  * [14] A. Rodriguez and M. T. Mason, “Path connectivity of the free space,” _IEEE Transactions on Robotics_ , vol. 28, no. 5, pp. 1177–1180, 2012.
  * [15] K. M. Lynch and M. T. Mason, “Stable pushing: Mechanics, controllability, and planning,” _The international journal of robotics research_ , vol. 15, no. 6, pp. 533–556, 1996.
  * [16] M. T. Mason, “Mechanics and planning of manipulator pushing operations,” _The International Journal of Robotics Research_ , vol. 5, no. 3, pp. 53–71, 1986.
  * [17] F. R. Hogan, E. R. Grau, and A. Rodriguez, “Reactive planar manipulation with convex hybrid mpc,” in _2018 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2018, pp. 247–253.
  * [18] N. Doshi, F. R. Hogan, and A. Rodriguez, “Hybrid differential dynamic programming for planar manipulation primitives,” in _2020 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2020, pp. 6759–6765.
  * [19] M. E. Houle and G. T. Toussaint, “Computing the width of a set,” in _Proceedings of the first annual symposium on Computational geometry_ , 1985, pp. 1–7.
  * [20] D. Dolgov, S. Thrun, M. Montemerlo, and J. Diebel, “Path planning for autonomous vehicles in unknown semi-structured environments,” _The international journal of robotics research_ , vol. 29, no. 5, pp. 485–501, 2010.
  * [21] C. Cao, H. Zhu, F. Yang, Y. Xia, H. Choset, J. Oh, and J. Zhang, “Autonomous exploration development environment and the planning algorithms,” in _2022 International Conference on Robotics and Automation (ICRA)_ , 2022, pp. 8921–8928.
  * [22] F. Zehra, M. Javed, D. Khan, and M. Pasha, “Comparative analysis of c++ and python in terms of memory and time. 2020,” _Preprints.[Google Scholar]_ , 2020.
  * [23] P. Anderson, A. Chang, D. S. Chaplot, A. Dosovitskiy, S. Gupta, V. Koltun, J. Kosecka, J. Malik, R. Mottaghi, M. Savva _et al._ , “On evaluation of embodied navigation agents,” _arXiv preprint arXiv:1807.06757_ , 2018.