SceneFactory: A Workflow-centric and Unified Framework for Incremental Scene Modeling

Yijun Yuan, Michael Bleier, Andreas Nüchter

The authors are with Informatics XVII – Robotics at Julius-Maximilians-University of Würzburg, Germany.
{yijun.yuan|michael<EMAIL_ADDRESS>
We present SceneFactory, a workflow-centric and unified framework for incremental scene modeling, that supports conveniently a wide range of applications,
such as (unposed and/or uncalibrated) multi-view depth estimation, LiDAR completion, (dense) RGB-D/RGB-L/Mono//Depth-only reconstruction and SLAM.
The workflow-centric design uses multiple blocks as the basis for building different production lines.
The supported applications, i.e., productions avoid redundancy in their designs.
Thus, the focus is on each block itself for independent expansion.
To support all input combinations,
our implementation consists of four building blocks in SceneFactory: (1) Mono-SLAM, (2) depth estimation, (3) flexion and (4) scene reconstruction.
Furthermore, we propose an unposed & uncalibrated multi-view depth estimation model (U$^2$-MVD) to estimate dense geometry.
U$^2$-MVD exploits dense bundle adjustment for solving for poses, intrinsics, and inverse depth.
Then a semantic-awared ScaleCov step is introduced to complete the multi-view depth.
Relying on U$^2$-MVD, SceneFactory both supports user-friendly 3D creation (with just images) and bridges the applications of Dense RGB-D and Dense Mono.
For high quality surface and color reconstruction, we propose due-purpose Multi-resolutional Neural Points (DM-NPs) for the first surface accessible Surface Color Field design, where we introduce Improved Point Rasterization (IPR) for point cloud based surface query.
We implement and experiment with SceneFactory to demonstrate its broad practicability and high flexibility.
Its quality also competes or exceeds the tightly-coupled state of the art approaches in all tasks. We contribute the code to the community[<https://jarrome.github.io/> The final link will be included after acceptance.].

Reconstruction, 3D Modelling, SLAM, RGBD

§ INTRODUCTION

Simultaneous Localization and Mapping (SLAM) plays an important role in robotics.
Previous works have made substantial progress for robust localization of robots [1, 2, 3], including in challenging environments [4], while the mapping of the environment is often sparse.

With the aim of producing higher quality maps, dense mapping [5] and 3D reconstruction [6] have been developed.
Dense mapping provides a point set as a mapping representation, but is much denser.
Alternatively, 3D reconstruction [6] produces meshes for compactness and continuity reasons, which is more user friendly.

However, we observe that most works construct the whole pipeline in a high tight-compact design.
This restricts the model to a specific application and is difficult to upgrade each submodule.
So this initiates our first research question: Can we design a framework that will support all of the production lines?

SceneFactory is workflow-centric and supports a wide range of applications given different input combinations of RGB $\V I_\text{rgb}$, depth $\V I_\text{d}$, pose $\V G$ and intrinsics $\V \theta$.

First of all, we do a traversal of all possible input cases and have a look at what the tasks might be.
As shown in <ref>, given different input combinations of RGB $\V I_\text{rgb}$, depth $\V I_\text{d}$, pose $\V G$ and intrinsics $\V \theta$, a number of applications could be run.
And our goal at the very beginning is to support them all in our single framework.

To achieve this goal, we design our framework in a workflow-centric way.
The advantage of such a design philosophy is that all tasks are connected by a dependency graph to reduce redundancy.
For example, as in our design <ref>, the final task usually depends on the other tasks.
However, the other tasks can also be the final task.
All subtasks work both independently and together, each block must be complete on its own.

Thus, this brings us to four blocks in the workflow: tracker, density estimator, chromaticity estimator and reconstructor.

Dependency graph for SceneFactory. The solid line indicate strict dependency, while dash line indicate optional dependency. The links between application and blocks follows <ref> and are not drawn. When pose $\V G$/RGB $\V I_\text{rgb}$/depth $\V I_\text{d}$ is given, their dependency is filled without additional calling.

On the tracking block, we subtract and find the greatest common divisor, Mono-SLAM.

Mono-SLAM considers more complicated conditions than RGB-D and Stereo SLAM because of the difficult depth estimation.
Which operates the simplest sensor setup with less observation demands.
However, Mono-SLAM relies on only sparse keypoints, and thus we further introduce a dense estimation task for dense depth-related applications.

The “loose connection” concept directs us to build a dense SLAM where each module focuses on its own task.

This is followed by dense estimation, which is widely studied under the topic of dense monocular SLAM.

Dense monocular SLAM is one of the most challenging branches of SLAM algorithms.
To estimate the dense information, researchers currently rely on encoder-decoder to generate and then optimise the mono-depth code [7, 8], use deep multi-view stereo (MVS) for direct depth estimation [9], or estimate using optical flow to provide the dense correlation between keyframes [10, 11].
However, mono-depth naturally loses scale and intrinsic information, usually resulting in distorted structures.
For MVS nets, they are overfitted with the trained camera intrinsic. They will be dysfunctional for custom cameras and new scenes.
Therefore, we turn to optical flow for correlation search and use dense bundle fitting to estimate the dense structure.
Our dense estimation design is also influenced by the observation that DROID-SLAM [10] and Sigma-Fusion [11] show much depth with a cone-like distortion.
Focusing on the cone distortion, we make an analysis and provide a different solution than Rosinol et al. [11].
We find that the problems come from the non-covisible regions, the low texture regions and the in-plane epipole.
For the first two, we find it is not the problem of the optical flow, because the optical flow is designed for tracking points, which does not assume a static environment.
But for static, i.e., rigid scenes, this is detrimental for both pose and density estimation.
Therefore, our first innovation is to add a static check to the optical flow based dense estimation.
Second, for the in-plane epipole, since it cannot be shielded by static check, we simply precompute possible epipoles and remove related matches.
However, the side effect of the above improvements will be missing regions in the depth image.

For use in SLAM sequences, we further introduce good neighborhood selection aiming to reduce the above situations.
But even with this selection, the side effect of producing missing regions still exists.
An example is in <ref>, where there is a hole in the wall.
Inspired by MonoSDF [12], that the monocular depth shares point properties with the real depth, we turn to monodepth and find in our scene case that although monodepth is distorted between structures, in-structure coplanarity is preserved.
Therefore, we propose to utilize this in-structure property with ScaleCov model.
That relies on learned deep covariance to fill the clipped locations with regressed scale image.
In SLAM related application, we additionally scale the depth to global to make the mapping consistent.

Even though our depth estimation model fits well into the Mono-SLAM, the model on its own supports unposed & uncalibrated multi-view depth estimation.
Which further provides a user-friendly application that only RGB images are required.

Afterwards, for immersive visualization in 3D reconstruction, there have been some successful attempts recently, such as ESLAM [13] and NeRF-SLAM [14].
They borrow the NeRF-like training scheme to produce high quality view synthesis.
However, the ray integration naturally suffers from many samples with large space and time cost.

Therefore, we turn to another related branch, Neural Surface Light Fields (NSLF), without the optimization of geometry.

Recent research of NSLF has verified the efficiency on SLAM sequences [15].
Because NSLF only learns surface field, each surface position is not modified.
But, the existing online SLF method [15] requires an external SDF model [16] to provide the surface points.

Therefore, we propose here due-purpose multi-resolutional neural points (DM-NPs) to overcome this limitation.
Following NSLF-OL [15], which use a multi-resolutional hashgrid for efficient online learning, we create a multi-resolutional point grid for fast SLF learning.
Moreover, since our point-based representation naturally provides position on surfaces, a straightforward idea is to use point rasterization.
However, point rasterization is a dysfunction compared to surface rasterization (as introduced in <ref>).
To make it work, we propose Improved Point Rasterization to make it really useful compared to Pytorch3D[17]'s point rasterization.
What's more, our CUDA implementation is also 10 to 50 times faster than Pytorch3D's CUDA implementation.

In summary, as shown in <ref>, we intend to design a modular scene modeling methods that each module supports individual applications, while it can be combined into ones.

The contributions of this work are:

* We propose a workflow-centric framework, SceneFactory, that supports all incremental scene modeling applications with different combination under the connection of a dependency graph.

* We propose a dual-purpose multiresolution neural points representation for both Surface Light Fields (SLF) and Improved Point Rasterization (IPR). Which is (1) the first surface-gettable SLF model, (2) the first to make point rasterization as usable as surface rasterization.

* Propose a robust depth estimation block with (1) an unposed & uncalibrated multiview depth estimation model (U$^2$-MVD) and (2) a deep correlation kernel based depth completion model, ScaleCov.

* We capture the first dense mono-SLAM purpose RGB-X dataset for high quality monocular reconstruction.

In the following, we first describe the related work on dense monocular SLAM, neural rendering in SLAM and multi-view depth estimation.
Then, in three separate sections, we introduce the due-purpose multiresolutional neural points representation, the unposed & uncalibrated depth estimation, and the whole SceneFactory structure.
Afterwards, we conduct experiments to thoroughly evaluate the performance of the system.
Finally, we conclude this paper and attach the supplementary.

§ RELATED WORKS

§.§ Dense SLAM

Textureless walls impose sensing challenges, since they can hardly be mapped in a mono or multi-camera setup.
Thus, dense SLAM systems operate the dense depth from laser scanners or RGB-D sensors to approach a high quality surface reconstruction.
When depth is involved, traditional fusion methods [18] incrementally update the Truncated Signed Distance Field (TSDF) and then extract afterwards a mesh using Marching Cubes [19].
More recently, neural priors are used for Neural Implicit Representation [16, 20].

Uni-Fusion's implicit representation supports even more data properties without any training [21].
With the even more recent trend of volumetric rendering, trained neural implicit rendering methods also come into view [22, 15].

However, the application scenario of scanners and depth sensors is still limited by their low affordability, portability and accessibility.
Alternatively, almost everyone has their own monocular camera at hand, e.g. in a mobile phone.
Therefore, there are still some works in this direction in the last years.

Optimizing an entire sequence of depths is not feasible given the large number of variables involved.
To reduce the computational costs of depth estimation,
CodeSLAM [7] and DeepFactors [8] optimize the latent codes of depth images.
However, the single-frame depth encoder-decoder needs to be pre-trained with similar data sets.

Instead of optimizing poses and depths at the same time, Tandem [9] first solves frame poses and then uses pre-trained MVSNet to recover the dense structure.

On the other hand, DROID-SLAM [10] ensembles dense optical flow modules into the pipeline, and optimizes poses and downsampled inverse depths using dense bundle adjustment.

To cope with DROID-SLAM's noisy depth estimation, Sigma-Fusion [11] introduces depth uncertainty into DROID-SLAM's framework and uses TSDF to provide a high quality dense reconstruction.

Starting in 2022, more researchers realized the high potential of using NeRF for high quality mapping.
Orbeez-SLAM [23] and NeRF-SLAM [24] first embedded NeRF in SOTA-SLAM frameworks.
For example, Orbeez-SLAM generates a sparse map using OrbSLAM3 [25] to set up a coarse occupancy grid for on-ray point sampling.
NeRF is then applied directly given the tracked poses.
Orbeez-SLAM gets the high quality rendering from NeRF.
But we find that it also inherits the problem of NeRF, especially for SLAM sequences.
That it can hardly work with non-around-object sequences.
While based on Sigma-Fusion, which provides dense depth in nature, NeRF-SLAM addresses the above problem by monitoring depth along with color.

§.§ Neural Rendering in SLAM

In our view, neural rendering is a topic that has the potential to replace reconstruction for high quality mapping.

Major group of 3D reconstruction mainly relies on marching cubes to extract mesh from explicit [18] or implicit [16, 20, 21].
This is not an efficient update due to the complicated (implicit)-to-explicit-to-mesh steps.
While the alternative view synthesis relies on rendering, shows a more direct way to extract visualization from implicit.
View generation could be super efficient.

The success of neural rendering in SLAM begins with the invention of iMAP [26] and NICE-SLAM [22].
iMAP applies volumetric rendering to MLPs and optimizes poses and MLPs with photometric loss.
NICE-SLAM, which improves the rendering base, replaces the MLPs with hierarchical feature grids.
This further improves the surface quality.
To avoid the problem we mentioned in Orbeez-SLAM, NICE-SLAM uses RGB-D sequences to provide depth information to avoid the convergence problem.
NICE-SLAM certainly provides a good basis for neural rendering.
However, it still has the problems that 1. it is not real-time capable, 2. the color result is of low quality.

To cope with this, Orbeez-SLAM and NeRF-SLAM give a solution by coupling SOTA-SLAM with SOTA-NeRF, instant-ngp [27].
On the other hand, NSLF-OL [15] focuses only on surface color, and produces real-time neural surface light field model to work together with an
external real-time reconstruction model.

However, working alongside make SLF's performance highly depends on the hosting reconstruction model.
Therefore, in this work, we also propose to make SLF model supports surface extraction.

§.§ Multi-view Depth Estimation

Depth from multi-view could be obtained from multiple ways, such as depth-from-video, multi-view stereo and pointmaps.

Depth-from-video directly estimates depth images and camera poses from video sequences with known camera intrinsics.
DeMoN [28] firstly utilize deep learning technique to estimate the depth and motion with a single network.
Not only operating on pair of images, DeepTAM [29] and DeepV2D [30] process more images with alternating mapping and tracking modules.
However, according to [31], such methods are overfitted to the trained scale and camera parameters.
Which are difficult to generalize to arbitrary real-world applications.

Multi-view stereo estimates 3D geometry from unconstrained images with given intrinsic and extrinsic information.
Here we only focus on the depth image estimation from MVS.
MVS is starting to get a boost with the trend of deep learning.
DeepMVS [32], as the first deep network based method, aggregates sampled patchwise deep features to estimate full image depth.
It has demonstrated the high potential of high quality depth estimation with deep learning.
Similarly, MVSNet [33] learns a feature map for whole images and fuses multi-view information into one feature map.
Then MVSNet's design serves as mainstream for following deep learning models [34, 35, 36, 37].

Pointmaps methods start attracting more attentions from 2024.
It uses dense 2D field of 3D points as geometric representation.
Pointmaps are first used in visual localization [38, 39] and monocular 3D reconstruction [40, 41].
A recent pointmaps-based work DUSt3R [42], designs a flexible stereo 3D reconstruction model.
DUSt3R groundbreakingly unifies different 3D tasks.
For the first time, it also provides a user-friendly interface that does not require camera parameters.

Our work is partly inspired by the seminal work DUSt3R.
On the one hand, we want to provide a user-friendly interface like DUSt3R.
On the other hand, we want to provide a unified model for all possible 3D related scene modeling.

Illustration of the multiresolution neural points in 2D.

§ DUAL-PURPOSES MULTIRESOLUTIONAL NEURAL POINTS

In this section, we introduce the dual-purpose representation that simultaneously supports Surface Light Fields (SLF) and point rasterization.
This representation is the first SLF method to support surface querying.

§.§ Multiresolutional Neural Points

Intuitively, a neural point is a point with a feature.
We denote a neural point as $\V v = (\V F_v, \V p_v)$ with feature $\V F_v\in\mathbb{R}^{m}$ and position $\V p_v\in\mathbb{R}^3$.

Then, inspired by instant-ngp [27], we use multiresolution neural points to improve learning efficiency.
These are multiple sets of neural points with different density.
We denote them as $\V V_a$ the set of $\V v$ with resolution/density $a$.

During training, we follow NSLF-OL [15] to feed the set of colored pairs ${(\V p_i, \V c_i)}_i$ for coding and training.
For each point position, we examine the density in its region and assign neural points.
Then the MLP coding is applied.

§.§.§ Neural Points Allocation

For the feedpoint set $\V Q=\{\V p_i\}_i$, given a set of resolutions $\V a$, we downsample to get $\{\V Q_a\}_{a\in\V a}$.
Then, for each point $\V p_i$ in $\V Q_a$, we check the closest distance to $a$ level neural points, if dist is greater than threshold $t_a$, the set of neural points is extended by adding this point and assigning it initialize with feature.

§.§.§ Surface Points Encoding

SLF's inference is on the surface, so this coding is an interpolation of the feature.

We draw a 2D illustration in <ref>.
For inference point $\V p\in\V Q$, we utilize efficient KDTree to find its $K$ nearest neighbor ($\{\V v^a_k\}_{k\in\{1,\cdots,K\}}$ with distance $\{d^a_k\}_{k\in\{1,\cdots,K\}}$) of inference point from each level.

The $a$ level feature is

\begin{equation}
F^a_{\V p}=\sum_{i\in\{1,\cdots,K\}} w_i F^a_{v_i}
\end{equation}

where $w_i= exp(-\frac{{d_i}^2}{\sigma})/ \sum_j exp(-\frac{{d_j}^2}{\sigma})$.

§.§.§ Color Prediction

Then the color prediction is generated by concatenating features along different levels and decoded with MLP: $f_\text{MLP}\circ f_\text{concat}((F^a_{\V p})_{a\in\V a})$.

§.§ Mapping via Online Learning

We learn this SLF in an online fashion by continuously feeding data according to [15].
A main thread of this module is to continuously train the SLF.
As soon as a new posed image with depth is input to the renderer, it is assigned a trained iteration, $it_\text{trained}$, to train in a least-touched strategy.

During the training iteration of a given frame, it randomly samples $n_\text{train}$ pixels and pass the corresponding point pairs $\{(\V p_n, \V c_n)_{n\in \{1,\cdots,n_\text{train}\}}\}$ to the neural allocation <ref> and prediction <ref> for the resulting color $(\V c^*_n)_n$.

We compute the MSE

\begin{equation}
L_\text{MSE} = \sum_{n\in \{1,\cdots,n_\text{train}\}}\parallel \V c^*_n - \V c_n  \parallel^2
\end{equation}

and use the Adam optimizer for stochastic gradient descent optimization of the features of neural points and MLP.

In addition, we use the following two strategy to speed up the training.

§.§.§ Jump-start Training Strategy

From the experiment, we find that our SLF converges slowly on the first frame.
While super fast for the following frames if the first frame gets rough color.
Otherwise, the other frames would also be relatively slow.

We think the problem is from the chaos of parameters as it is randomly initialized.
So we set a $10\times$ higher learning rate for the first $5$ iterations and recover then.
This way, even the first image converges in a second.

§.§.§ Least-trained First Training Strategy

The second training strategy is that after the first frame, when another frame is input, the previous well-trained frame shouldn't have the same chance to be trained as the new one.
So an intuitive way is to always train the least trained frame.

In this way, all subsequent frames are converged in one second.

§.§ Improved Point Rasterization (IPR)

Above is the training process where the depth is given.
During the inference, we have to estimate the depth.
Therefore, point rasterization is introduced.

For simplicity, we will now operate on points in camera space.
To render an image with a resolution of $H\times W$, we cast a batch of rays $(\V o_i=\V 0, \V d_i\in{\mathbb S^2})_{i\in\{1,\cdots,HW\}}$ with $(\V o, \V d)$ as the ray source and direction.

On the other hand, the positions of the neural points are first transformed into NDC space.

Set camera space point $\V p = [X,Y,Z]^T$ as an example, projected point in NDC is then $\V p_\text{ndc}=[f_x \frac{X}{Z} + p_x,
f_y \frac{ Y} { Z} + p_y, \frac{1} { Z}]^T$. Where $(f_x, f_y, p_x,p_y)$ are intrinsic parameters.
In this way, each ray is directed to $+z_\text{ndc}$, i.e., $\V d_\text{ndc} = [0,0,1]^T$. The source of the NDC space ray is obtained by projecting $\V d$ accordingly.

Points within a radius $r_\text{ndc}$ around each ray $(\V o_\text{ndc} = [x_\text{ndc}, y_\text{ndc},z_\text{ndc}]^T, \V d_\text{ndc})$ are then computed using only the $(x,y)$ coordinate.

Thus, for each ray $(\V o, \V d)_i$, we find its $K_\text{ray}$ nearest neural point neighbors and the distance $\{(\V p_{nb,k},d_{nb,k})\}_{k\in\{1,\cdots,K_\text{ray}\}}$ within the radius.
The rasterized point is thus

\begin{equation}
\V p_\text{raster}=\sum_{k\in\{1,\cdots,K\}} w_{nb, k}\V p_{nb,k}
\end{equation}

where $w_{nb,k}= exp(-\frac{{d_k}^2}{\sigma})      / \sum_j exp(-\frac{{d_j}^2}{\sigma})$.

However, unlike mesh rasterization, point rasterization suffers from the following problems:

* the point distribution in NDC space is distorted.
That is, the points near the camera are more sparse while the points far from the camera are more dense compared to Euclidean space.
This results in holes in the near region.

* the rasterization may contain several layers of points.

To deal with 1) the hole problem, we introduce an adaptive radius to PR. This changes the radius depending on the depth.

Considering that our finest resolution is $r=0.005m$ and the screen space is at $z=1$, we expect that each point with a $0.005m$ gap can completely cover a pixel at $z=1$.
The coverage radius should be $l_\text{coverage}=\sqrt{2}(r\cdot f_{im})$ where $f_im$ is the focal length parameter.
To simplify the implementation, our CUDA code creates a $l_\text{coverage}\times l_\text{coverage}$ window for this point.

If $z<1$, the scenario in <ref> can occur because the points are distorted in the screen space.
The ray will then penetrate the near surface.
To solve this problem, we increase the coverage area, the coverage length becomes $l_\text{coverage} = \sqrt{2}(r\cdot f_{im})/z$.

Improved Point Rasterization.

For the 2) problem of multiple layers, we propose to use first-layer detection to keep only the points of the first layer.
Given $n$ and $z$-axis ascending sorted points traced by a given ray, $\{\V z_0,\cdots z_n\}$, we compute the occupancy of point $z_t$.

\begin{equation}
\begin{aligned}
1\ \ \ \ \ \ \ \ & && t=0,\\
z_{t+1}-z_t < th& &&t>0. \\
\end{aligned}
\right.
\end{equation}

Then, by cumulating production, we get the mask of the first layer points:

\begin{equation}
o_{\{0,\cdots\}} = cumprod(o_{\{0,\cdots\}}).
\end{equation}

We implement our IPR with CUDA, which is about 20 times faster than Pytorch3d's CUDA implementation ([40]FPS vs [2]FPS).
We demonstrate the effect of IPR in Suppl.<ref>.

§.§ Visualization

This function plays an important role in rasterizing the depth image and extracting the surface point colors under certain viewpoints.
First, we rasterize at the level of neural points with the highest resolution.
Then, transforming back to world coordinates with $\V T$, the color prediction is obtained by

\begin{equation}
\V c_{raster} = f_{MLP}\circ f_{concat}((F^a_{\V T \V p_{raster}})_{a\in\V a}).
\end{equation}

We follow [15] to create another thread with an interactive GUI to provide a first-person view of the scene.

This thread gets a signal from the user to rotate and move the view camera.
The view synthesis is rendered in real time.

§ DEPTH ESTIMATION

Depth estimation via unposed & uncalibrated multi-view depth estimation model (U$^2$-MVD).

In this section, we introduce our unposed & uncalibrated multi-view depth estimation model (U$^2$-MVD) that is depicted as in <ref>.
As an adjunct to SLAM, we additionally add good neighbor frame selection for more suitable frames.

§.§ Depth Recovery with Dense Correlation

We acquire dense correspondences from SOTA optical flow estimation model DKMv3 [43] for pixel-wise correspondences $\V l_{i,j}\in\mathbb{R}^{H\times W\times2}$ between the frames $i$ and $j$.

Then the expected correspondence pixel would be $\V x_{j}^*=\V l_{i,j}+\V x_{i}$, where $\V x_{i}$ is the pixel coordinate in frame $i$.
However, if false correspondences occur, it will strongly affect the resulting pose and depth.
That is, the optical flow cannot be fully trusted.

§.§.§ Cross Check

Because single-source matching is risky, we apply cross-checking of correspondences to ensure a high quality match:

\begin{align}
\V w_{i,j,(u,v)}=
\begin{cases}
1               & \text{if } ||\V x_{i,(u,v)} - (\V x^*_{j,(u,v)}+ \V I_{j,i,(u,v)})||_2< 0.5, \\
0,              & \text{otherwise}
\end{cases}
\end{align}

where $(u,v)$ is the pixel coordinate in the image.

§.§.§ Static Check

We have assumed that our scene is static.
However, optical flow is not designed for static scenes.
That is, the flow is not constrained by the rigid body.
Therefore, unlike previous works [10] that trust the flow, we apply epipolar constraint to filter out the non-rigid flow:

\begin{align}
\V w_{i,j,(u,v)}=
\begin{cases}
1               & \text{if } dist_{line}(\V L_{j,(u,v)},\V x^*_{j,(u,v)})^2< 3.84, \\
0,              & \text{otherwise}
\end{cases}
\end{align}

where $\V L_{j,(u,v)}$ is the epipolar line for $\V x^*_{j,(u,v)}$, $dist_{line}$ is the distance from point to line.
The epipolar line $\V L_{j}$ is obtained by solving the essential matrix with $1000$ randomly chosen correspondences.

§.§.§ In-image Epipole Check

However, since the above static check relies on the epipolar constraint, this check does not work near the epipole, i.e., if the epipole is in the image, the near pixels will all pass the static check.

Therefore, we compute the epipole position in image and filter out the near pixel correspondences.

§.§.§ Dense Bundle Adjustment (DBA)

Although the SOTA optical flow is used, from experiment, single flow is still too fragile for dense reconstruction use.

Therefore, more source frames are usually involved in MVD tasks.
In our formulation, we project reference frame's depth to all source frames and supervise via dense correspondences between each reference-source pair ($\mathcal{B}_t$).
In connecting with SLAM, we also collect dense matching from reference (frame $t$) to neighbor frames ($\mathcal{B}_t$) in window, and compute dense BA to stabilize the inverse depth of reference frame $\V d_t$.

Then we follow DROID [10, 44] to denote the poses, the intrinsics and the projection function as $\V G$, $\V \theta$ and $\Pi$.
The cost function of the Dense Bundle Adjustment (DBA) would be the sum of the projection errors over all neighboring frames:

\begin{equation}
\label{eq:DBA}
\mathcal L(\V G, \V d_t, \V \theta) = \sum_{j \in \mathcal{B}_t} \parallel \V x^*_j - \Pi(\V G_{tj}\circ \Pi^{-1}(\V x_{t}, \V d_t, \V \theta),\V \theta) \parallel^2.
\end{equation}

This cost function is solved efficiently with Gaussian-Newton. For the detailed formulation, please refer to DROID-SLAM [10, 44].
<ref> directly solves poses, intrinsics and inverse depth at the same time with only optical flow (dense correspondences) as true value.

§.§.§ Monocular Depth and Depth Completion

In addition to the DBA, we utilize Metric3Dv2 [45] for monocular depth $\V {\hat{z}_t}$ on the reference frame $t$.
$\V {\hat{z}_t}$ could help the DBA from a good start point.
While more importantly, we use $\V {\hat{z}_t}$ to assist completing the DBA depth with vacancy.

Recent monocular depth methods [46, 45] provide high quality depth estimation of structures.
However, because the estimation from a single image naturally loses scale and intrinsic information, as in <ref>, the relative positions between different structures are distorted.
Besides, monodepth suffers vision illusion such as Ames room in <ref>.
Which make inter-structure of monodepth theoretically not reliable.

[width=.5][Front view]

[width=.5][Over view]

Monocular depth example.

Nevertheless, we still find useful information: the coplanarity is preserved.
Which reminds us to extract useful information from intra-structure.

Thus we intend to utilize learned covariance function that can semantically identify intra- and extra-structures.
We find that DepthCov [47] learns deep prior.
However, a problem of DepthCov is that it directly regresses depth image with true sparse depth observation.
This means that if there is no true sample on certain structure, the depth of that structure would be arbitrary wrong.
An extreme example is with only $1$ sample, then the whole depth image will be the same value.

Conversely, we introduce ScaleCov, which regresses scale image for monodepth $\V {\hat{z}_t}$ given DBA depth $\V {z_t}$ as observation.
ScaleCov does not face this problem, because in the worst case identical scale image does not ruin the depth structures.

Following DepthCov, we utilize its deep learned covariance function (kernel function) $K$ to formulate ScaleCov as

\begin{equation}
\V s_{*} = \V K_{fn}(\V K_{nn} + \sigma_n^2\V I)^{-1}\V {s}_n,
\end{equation}

\begin{equation}
\V \Sigma_{*} = \V K_{ff} - \V K_{fn}(\V K_{nn} + \sigma_n^2\V I)^{-1}\V K_{nf}.
\end{equation}

Where, we sample non-vacancy depth from DBA depth and compute scales $\V s_n=\V z_{t,n}/\V {\hat{z}}_{t,n}$, and use $\V K_{ff}$, $\V K_{fn}$ and $\V K_{nn}$ to denotes the correlation matrix between query points, query and observations, and observations.
$\V s_{*}$ and $\V \Sigma_{*}$ are the queried scale and variance  that would be associated with the depth image.

For the finished image we consider the depth to be complete is more trustworthy and let

\begin{align}
\V {\bar{s}}_t(\V x)=
\begin{cases}
\V z_{t}(\V x)/\V {\hat z}_{t}(\V x),& \text{if } \V z_t(\V x)> 0\\
\V s_{*}(\V x),              & \text{otherwise}
\end{cases}
\end{align}

\begin{align}
\V {\bar{\Sigma}}_t(\V x)=
\begin{cases}
\V 0,& \text{if } \V z_t(\V x)> 0\\
\V \Sigma_{*}(\V x),              & \text{otherwise}
\end{cases}
\end{align}

where $\V x$ the pixel coordinate.

The final completed depth is

\begin{equation}
\V {\bar{z}}_t = \V {\bar{s}}_t * \V {\hat{z}}_t
\end{equation}

where the corresponding pixel-wise variance is $\V {\bar{\Sigma}}_t$.

In addition to the MVD completion shown in <ref>, ScaleCov can also support sparse LiDAR depth completion as shown in <ref>.

§.§ Depth Frame Selection

We observe that all previous dense mono SLAMs estimate global geometry using keyframes.
It makes sense to estimate depth for keyframes because during the tracking process, the main burden of keyframes is to gain information.
The goal is to keep the landmarks in view.

However, we find it problematic to use neighbor keyframes to support depth recovery.
This does not guarantee a good basis for triangulation, especially in dense cases.

Pose and depth computation.

Therefore, as abstracted in <ref>, the good neighbor frames (NF) may not be keyframes (KF).
In addition, the tracking model stores non-KF poses as a relative pose to the previous KF.
This may not be accurate.
While using our depth model, we only input intrinsics and images to estimate the depth without using the poses of the NFs.

Next, we ask the question: “How to choose a frame that is good for depth estimation?”

Reliable depth estimation requires a reference frame for triangulation.
Therefore, our answer is: “A frame has a good neighborhood for triangulation”.

Because the tracking model provides poses for all frames, we can use the relative poses to the reference frame to help verify.
We select neighboring frames according to <ref>, where the relative facing angle and baseline are used.

§.§ Recover Scale to Global

Since <ref> is applied without poses, a scale recovery operation is required to adjust to the tracking scale.

While the global scale is hidden in the tracked landmarks or provided poses.

When landmarks are given, the relative scale from local to global is recovered by using RANSAC regression[<https://scikit-learn.org/stable/modules/generated/sklearn.linear_model.RANSACRegressor.html>]
$s_{t,g}=RANSACRegressor(\{\V z_t^l(\V x)/\V {\bar{z}}_t(\V x)*s=1\}_{\V x\in \mathcal{P}_t})$, where $\V z_t^l$ and $\V {\bar{z}}_t$ are the landmarks from the tracker and the estimated depth from <ref>, $\mathcal{P}_t$ and $\V x$ are the set of landmarks in frame $t$ and pixel coordinate.

When landmarks are missing (use provided poses), we adjust depth with the scale difference between solved and provided poses.

The dependency graph in SceneFactory.
The yellow line indicates the dependency edge when the value is None.
The green, purple, blue, and pink lines indicate the dependency edge of the Bending, Depth, Tracking, and Scene Reconstruction blocks.
The gray line indicates the requirement of a specific application.
The solid/dotted line shows mandatory/optional dependency.

Inside of each module, the black arrow indicates the pipeline.

§ A UNIFIED FRAMEWORK FOR INCREMENTAL SCENE MODELING

SceneFactory supports various combinations of $\V I_{rgb}$, $\V I_{d}$, $\V G$ and $ \V \theta$ as input.
We plot the entire pipeline in <ref> to have a brief overview.
All of the applications in <ref> can find their corresponding parts in this diagram.

SceneFactory consists of four building blocks:

* flexion estimation block to the complete the image,

* depth estimation block to estimate and complete the depth,

* tracking block to complete pose,

* scene reconstructing block.

In this section, we introduce each of these blocks respectively (<ref>) and combine them to a whole workflow (<ref>).

§.§ Tracking Block

For the tracking block, we use Mono-SLAM because it is a greatest common divisor with minimal demands.
More specifically, our tracking block is based on DPVO [48], while we generalize it to also to support RGB-D/L input.

DPVO is based on sparse correspondences, while the pose $\V G$ is optimized with sparse bundle adjustment:

\begin{equation}
\mathcal L_{track}(\V G, \V P) = \sum_{(k,j) \in \mathcal{E}} \parallel \Pi(\V G_{ij}\circ \Pi^{-1}(\V P^{'}_{k}))
%\hat\omega_{ij}(\V G_j, \V P^{'}_{kj})
- [\hat{\V P}^{'}_{kj}+\delta_{kj}] \parallel_{\Sigma_{kj}}^2.
\end{equation}

where $\V P^{'}_{k}$ is the patch $k$ in image $i$,  $\hat{\V P}^{'}_{kj}$ is the center of patch $\V P^{'}_{kj}$ in image $j$, $\delta_{kj}$ is the patch update.

The above formulation is solved efficiently with Gauss Newton.
When the depth $\V I_d$ is given, we extract patch $\V P$ from the corresponding position.
We fix the patch from valid depth pixel and optimize only the poses $\V G$ from $\mathcal L_{track}$ with Gauss Newton.
We choose DPVO for the generalization because it does not require explicit extraction of matches, and thus well fit the sparse observation of the LiDAR.

From our experiment, this generalized version is more accurate by utilizing the metric depth.

§.§ Flexion Estimation Block

[Colorized depth]

Depth images and their correspondingly converted flexion images.

Flexion estimation is required when $\V I_{rgb}$ is missing (depth only).

To support depth-only tracking in our RGBD SLAM framework, a feature matching operation is required. However, the depth image does not contain the SE3-invariant context for descriptor extraction.

Therefore, we use the seminal depth converter [49] to convert the depth image into a flexion image that has the SE3-invariant properties.
An example is given in <ref>. We colorize the depth only for better visualization. We see that flexion images contain a more consistent value, which is more suitable for feature matching [49].

Therefore, depth-only applications are able to treat flexion as RGB and serve as RGBD applications as in <ref>.

§.§ Depth Estimation Block

The depth estimation block is mainly our U$^2$-MVD (in <ref>) with intrinsics given for pose-free MVD to support incremental application.
It fills the depth void for RGB applications, allowing them to work as RGB-D applications.

Note that when no metric depth is specified, tracking frames only requires sparse-BA with images.
Depth estimation is only required when KF and NFs are detected or MVD is called for certain application requests.
Once requested, this block will query memory for good neighbor selection (<ref>), pose-free MVD (<ref>), depth completion (<ref>) and scale recovery (<ref>).

We put the depth estimation behind the tracking sequences because the tracker's sparse BA will also optimize poses in a local window, the updated landmarks will affect the scale recovery.

§.§ Reconstruction Block

The reconstruction block uses our DM-NPs presented in <ref>.
This block maintains two threads: 1) NPs allocation and online learning, and 2) visualization.

§.§.§ Online-learning thread

A training thread is used to continuously train our SLF model.
As soon as a new keyframe with ($\V I_{rgb}^{'}$, $\V I_d^{'}$, $\V G^{'}$) is input to the renderer, it is assigned a trained iteration, $it_{trained}$, to train in a least-touch-first strategy.

During the training iteration of a given frame, it will randomly sample $n_{train}$ pixels and pass the corresponding point pairs $\{(\V p_n, \V c_n)_{n\in \{1,\cdots,n_{train}\}}\}$ to the neural allocation <ref> and prediction <ref> for the resulting color $(\V c^*_n)_n$.

We compute the MSE

\begin{equation}
\mathcal L_{MSE} = \sum_{n\in \{1,\cdots,n_{train}\}}\parallel \V c^*_n - \V c_n  \parallel^2
\end{equation}

and use the Adam optimizer for a stochastic gradient descent optimization of neural point features and MLP.

§.§.§ Visualization thread

Another thread for the renderer is to provide an interactive GUI to provide a first person view of the scene.

This thread gets a signal from the user to rotate and move the view camera.
The view synthesis is rendered in real time (<ref>).

§.§ Main Function

SceneFactory's main functions are $EstablishProductLine$ and $Step$ as in <ref>.
$(\V I_{rgb}$, $\V I_d$, $\V G$, $\V \theta$, $app )$
Build product line
$pLine=AssemblingParts(\V I_{rgb}$, $\V I_d$, $\V G$, $\V \theta$, $app )$;

End Function

$(\V I_{rgb}$, $\V I_d$, $\V G$, $\V \theta$, $app )$
Product line start working part-by-part
$\V V_{inter}\leftarrow pLine.Package(\V I_{rgb}$, $\V I_d$, $\V G$, $\V \theta$, $app )$;

$f_{part}$ in pLine.parts
$\V V_{inter}=f_{part}(\V V_{inter})$
$pLine.Unpackage(\V V_{inter})$;

End Function
Main functions

When a task is triggered,
SceneFactory successively checks the availability of image $\V I_{rgb}$, pose $\V G$, depth $\V I_d$ and intrinsics $\V \theta$, to prepare the product line ready.
Then the operation are conducted following <ref>.

For example, if $\V I_{rgb}$ is missing (depth only), flexion block (<ref>) is added to the product line to estimate the trackable image from depth.
If SceneFactory is required to solve intrinsics without a $\V \theta$ input, it will add intrinsic-free DBA to the line for $\V \theta$.
If SceneFactory is asked to solve poses without poses $\V G$ as input, it will add a tracker to the line for predicting the pose $\V G^{'}$.
If the metric depth is missing,
depth estimator block (<ref>) will be added for estimating the depth.
While if the metric depth input is sparse (from LiDAR), only the depth completion part will be added.
When reconstruction is requested by the application, the completed frame parameters with image, depth, pose and intrinsics are required to be fed into the reconstruction block (<ref>) for online learning and visualization.
And corresponding parts are all be added to the product line.

Then the product line treats each step function as a task. If a single task is requested (such as MVD, Completion and etc.), production line will return the step result as a product.
While if sequential application is requested (such as SLAM, Reconstruction and etc.), product line will conditionally step on each frame for the intermediate and return product at the end of this production process.

§ EXPERIMENTS

The main experiments are conducted on multi-view depth estimation, surface light field and Dense SLAM.

§ CONCLUSION

In this paper, we have introduced a workflow-centric and unified framework for incremental scene modeling, called SceneFactory.
Following the structure of a “Factory”, we have designed “assembly lines” for a wide range of applications, to achieve high flexibility, adaptability and production diversification.

In addition, within the framework, we propose an unposed & uncalibrated multi-view depth estimation model for high flexible use.
We introduce surface accessible surface light field design together with improved point rasterization to enable the surface query for the first time.

Our experiments show that SceneFactory is highly competitive or even better than compact SOTAs in such applications.
The high quality and broad applicability of the design further enhances the progress of our design.

Needless to say, a lot of work remains to be done. In future work, we will concentrate on adding more applications to the production lines, e.g., deformable reconstruction, active SLAM, or scene understanding [59].

[1]
J. Engel, T. Schöps, and D. Cremers, “Lsd-slam: Large-scale direct
monocular slam,” in European conference on computer vision.1em
plus 0.5em minus 0.4emSpringer, 2014, pp. 834–849.

[2]
R. Mur-Artal and J. D. Tardós, “Orb-slam2: An open-source slam system for
monocular, stereo, and rgb-d cameras,” IEEE transactions on robotics,
vol. 33, no. 5, pp. 1255–1262, 2017.

[3]
C. Campos, R. Elvira, J. J. G. Rodríguez, J. M. Montiel, and J. D.
Tardós, “Orb-slam3: An accurate open-source library for visual,
visual–inertial, and multimap slam,” IEEE Transactions on Robotics,
vol. 37, no. 6, pp. 1874–1890, 2021.

[4]
K. Ebadi, L. Bernreiter, H. Biggie, G. Catt, Y. Chang, A. Chatterjee, C. E.
Denniston, S.-P. Deschênes, K. Harlow, S. Khattak, L. Nogueira, M. Palieri,
P. Petráček, M. Petrlík, A. Reinke, V. Krátký, S. Zhao, A.-a.
Agha-mohammadi, K. Alexis, C. Heckman, K. Khosoussi, N. Kottege, B. Morrell,
M. Hutter, F. Pauling, F. Pomerleau, M. Saska, S. Scherer, R. Siegwart, J. L.
Williams, and L. Carlone, “Present and Future of SLAM in Extreme
Environments: The DARPA SubT Challenge,” IEEE Transactions on
Robotics, vol. 40, pp. 936–959, 2024.

[5]
C. Kerl, J. Sturm, and D. Cremers, “Dense visual slam for rgb-d cameras,” in
2013 IEEE/RSJ International Conference on Intelligent Robots and
Systems.1em plus 0.5em minus 0.4emIEEE, 2013, pp. 2100–2106.

[6]
R. A. Newcombe, S. Izadi, O. Hilliges, D. Molyneaux, D. Kim, A. J. Davison,
P. Kohi, J. Shotton, S. Hodges, and A. Fitzgibbon, “Kinectfusion: Real-time
dense surface mapping and tracking,” in 2011 10th IEEE international
symposium on mixed and augmented reality.1em plus 0.5em minus
0.4emIeee, 2011, pp. 127–136.

[7]
M. Bloesch, J. Czarnowski, R. Clark, S. Leutenegger, and A. J. Davison,
“Codeslam—learning a compact, optimisable representation for dense visual
slam,” in Proceedings of the IEEE conference on computer vision and
pattern recognition, 2018, pp. 2560–2568.

[8]
J. Czarnowski, T. Laidlow, R. Clark, and A. J. Davison, “Deepfactors:
Real-time probabilistic dense monocular slam,” IEEE Robotics and
Automation Letters, vol. 5, no. 2, pp. 721–728, 2020.

[9]
L. Koestler, N. Yang, N. Zeller, and D. Cremers, “Tandem: Tracking and dense
mapping in real-time using deep multi-view stereo,” in Conference on
Robot Learning.1em plus 0.5em minus 0.4emPMLR, 2022, pp.

[10]
Z. Teed and J. Deng, “Droid-slam: Deep visual slam for monocular, stereo, and
rgb-d cameras,” Advances in neural information processing systems,
vol. 34, pp. 16 558–16 569, 2021.

[11]
A. Rosinol, J. J. Leonard, and L. Carlone, “Probabilistic volumetric fusion
for dense monocular slam,” in Proceedings of the IEEE/CVF Winter
Conference on Applications of Computer Vision, 2023, pp. 3097–3105.

[12]
Z. Yu, S. Peng, M. Niemeyer, T. Sattler, and A. Geiger, “Monosdf: Exploring
monocular geometric cues for neural implicit surface reconstruction,”
Advances in neural information processing systems, vol. 35, pp.
25 018–25 032, 2022.

[13]
M. M. Johari, C. Carta, and F. Fleuret, “Eslam: Efficient dense slam system
based on hybrid representation of signed distance fields,” in
Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern
Recognition, 2023, pp. 17 408–17 419.

[14]
A. Rosinol, J. J. Leonard, and L. Carlone, “Nerf-slam: Real-time dense
monocular slam with neural radiance fields,” arXiv preprint
arXiv:2210.13641, 2022.

[15]
Y. Yuan and A. Nüchter, “Online learning of neural surface light fields
alongside real-time incremental 3d reconstruction,” IEEE Robotics and
Automation Letters, 2023.

[16]
J. Huang, S.-S. Huang, H. Song, and S.-M. Hu, “Di-fusion: Online implicit 3d
reconstruction with deep priors,” in Proceedings of the IEEE/CVF
Conference on Computer Vision and Pattern Recognition, 2021, pp. 8932–8941.

[17]
N. Ravi, J. Reizenstein, D. Novotny, T. Gordon, W.-Y. Lo, J. Johnson, and
G. Gkioxari, “Accelerating 3d deep learning with pytorch3d,” arXiv
preprint arXiv:2007.08501, 2020.

[18]
R. A. Newcombe, S. Izadi, O. Hilliges, D. Molyneaux, D. Kim, A. J. Davison,
P. Kohi, J. Shotton, S. Hodges, and A. Fitzgibbon, “Kinectfusion: Real-time
dense surface mapping and tracking,” in 2011 10th IEEE international
symposium on mixed and augmented reality.1em plus 0.5em minus
0.4emIeee, 2011, pp. 127–136.

[19]
W. E. Lorensen and H. E. Cline, “Marching cubes: A high resolution 3d surface
construction algorithm,” in Seminal graphics: pioneering efforts that
shaped the field, 1998, pp. 347–353.

[20]
Y. Yuan and A. Nüchter, “An algorithm for the se (3)-transformation on
neural implicit maps for remapping functions,” IEEE Robotics and
Automation Letters, vol. 7, no. 3, pp. 7763–7770, 2022.

[21]
——, “Uni-fusion: Universal continuous mapping,” IEEE Transactions
on Robotics, 2024.

[22]
Z. Zhu, S. Peng, V. Larsson, W. Xu, H. Bao, Z. Cui, M. R. Oswald, and
M. Pollefeys, “Nice-slam: Neural implicit scalable encoding for slam,” in
Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern
Recognition, 2022, pp. 12 786–12 796.

[23]
C.-M. Chung, Y.-C. Tseng, Y.-C. Hsu, X.-Q. Shi, Y.-H. Hua, J.-F. Yeh, W.-C.
Chen, Y.-T. Chen, and W. H. Hsu, “Orbeez-slam: A real-time monocular visual
slam with orb features and nerf-realized mapping,” in 2023 IEEE
International Conference on Robotics and Automation (ICRA).1em plus
0.5em minus 0.4emIEEE, 2023, pp. 9400–9406.

[24]
A. Rosinol, J. J. Leonard, and L. Carlone, “Nerf-slam: Real-time dense
monocular slam with neural radiance fields,” in 2023 IEEE/RSJ
International Conference on Intelligent Robots and Systems (IROS).1em
plus 0.5em minus 0.4emIEEE, 2023, pp. 3437–3444.

[25]
C. Campos, R. Elvira, J. J. G. Rodríguez, J. M. Montiel, and J. D.
Tardós, “Orb-slam3: An accurate open-source library for visual,
visual–inertial, and multimap slam,” IEEE Transactions on Robotics,
vol. 37, no. 6, pp. 1874–1890, 2021.

[26]
E. Sucar, S. Liu, J. Ortiz, and A. J. Davison, “imap: Implicit mapping and
positioning in real-time,” in Proceedings of the IEEE/CVF
International Conference on Computer Vision, 2021, pp. 6229–6238.

[27]
T. Müller, A. Evans, C. Schied, and A. Keller, “Instant neural graphics
primitives with a multiresolution hash encoding,” ACM transactions on
graphics (TOG), vol. 41, no. 4, pp. 1–15, 2022.

[28]
B. Ummenhofer, H. Zhou, J. Uhrig, N. Mayer, E. Ilg, A. Dosovitskiy, and
T. Brox, “Demon: Depth and motion network for learning monocular stereo,”
in Proceedings of the IEEE conference on computer vision and pattern
recognition, 2017, pp. 5038–5047.

[29]
H. Zhou, B. Ummenhofer, and T. Brox, “Deeptam: Deep tracking and mapping,” in
Proceedings of the European conference on computer vision (ECCV),
2018, pp. 822–838.

[30]
Z. Teed and J. Deng, “Deepv2d: Video to depth with differentiable structure
from motion,” in International Conference on Learning
Representations, 2019.

[31]
P. Schröppel, J. Bechtold, A. Amiranashvili, and T. Brox, “A benchmark and
a baseline for robust multi-view depth estimation,” in 2022
International Conference on 3D Vision (3DV).1em plus 0.5em minus
0.4emIEEE, 2022, pp. 637–645.

[32]
P.-H. Huang, K. Matzen, J. Kopf, N. Ahuja, and J.-B. Huang, “Deepmvs: Learning
multi-view stereopsis,” in Proceedings of the IEEE conference on
computer vision and pattern recognition, 2018, pp. 2821–2830.

[33]
Y. Yao, Z. Luo, S. Li, T. Fang, and L. Quan, “Mvsnet: Depth inference for
unstructured multi-view stereo,” in Proceedings of the European
conference on computer vision (ECCV), 2018, pp. 767–783.

[34]
Y. Yao, Z. Luo, S. Li, T. Shen, T. Fang, and L. Quan, “Recurrent mvsnet for
high-resolution multi-view stereo depth inference,” in Proceedings of
the IEEE/CVF conference on computer vision and pattern recognition, 2019,
pp. 5525–5534.

[35]
J. Yang, W. Mao, J. M. Alvarez, and M. Liu, “Cost volume pyramid based depth
inference for multi-view stereo,” in Proceedings of the IEEE/CVF
conference on computer vision and pattern recognition, 2020, pp. 4877–4886.

[36]
X. Gu, Z. Fan, S. Zhu, Z. Dai, F. Tan, and P. Tan, “Cascade cost volume for
high-resolution multi-view stereo and stereo matching,” in Proceedings
of the IEEE/CVF conference on computer vision and pattern recognition, 2020,
pp. 2495–2504.

[37]
J. Zhang, S. Li, Z. Luo, T. Fang, and Y. Yao, “Vis-mvsnet: Visibility-aware
multi-view stereo network,” International Journal of Computer Vision,
vol. 131, no. 1, pp. 199–214, 2023.

[38]
E. Brachmann, A. Krull, S. Nowozin, J. Shotton, F. Michel, S. Gumhold, and
C. Rother, “Dsac-differentiable ransac for camera localization,” in
Proceedings of the IEEE conference on computer vision and pattern
recognition, 2017, pp. 6684–6692.

[39]
J. Revaud, Y. Cabon, R. Brégier, J. Lee, and P. Weinzaepfel, “Sacreg:
Scene-agnostic coordinate regression for visual localization,” arXiv
preprint arXiv:2307.11702, 2023.

[40]
C.-H. Lin, C. Kong, and S. Lucey, “Learning efficient point cloud generation
for dense 3d object reconstruction,” in proceedings of the AAAI
Conference on Artificial Intelligence, vol. 32, no. 1, 2018.

[41]
J. Wang, B. Sun, and Y. Lu, “Mvpnet: Multi-view point regression networks for
3d object reconstruction from a single image,” in Proceedings of the
AAAI Conference on Artificial Intelligence, vol. 33, no. 01, 2019, pp.

[42]
S. Wang, V. Leroy, Y. Cabon, B. Chidlovskii, and J. Revaud, “Dust3r: Geometric
3d vision made easy,” arXiv preprint arXiv:2312.14132, 2023.

[43]
J. Edstedt, I. Athanasiadis, M. Wadenbäck, and M. Felsberg, “Dkm: Dense
kernelized feature matching for geometry estimation,” in Proceedings
of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, 2023,
pp. 17 765–17 775.

[44]
A. Hagemann, M. Knorr, and C. Stiller, “Deep geometry-aware camera
self-calibration from video,” in Proceedings of the IEEE/CVF
International Conference on Computer Vision, 2023, pp. 3438–3448.

[45]
M. Hu, W. Yin, C. Zhang, Z. Cai, X. Long, H. Chen, K. Wang, G. Yu, C. Shen, and
S. Shen, “A versatile monocular geometric foundation model for zero-shot
metric depth and surface normal estimation,” 2024.

[46]
W. Yin, C. Zhang, H. Chen, Z. Cai, G. Yu, K. Wang, X. Chen, and C. Shen,
“Metric3d: Towards zero-shot metric 3d prediction from a single image,” in
Proceedings of the IEEE/CVF International Conference on Computer
Vision, 2023, pp. 9043–9053.

[47]
E. Dexheimer and A. J. Davison, “Learning a depth covariance function,” in
Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern
Recognition, 2023, pp. 13 122–13 131.

[48]
Z. Teed, L. Lipson, and J. Deng, “Deep patch visual odometry,” Advances
in Neural Information Processing Systems, vol. 36, 2024.

[49]
R. Lösch, M. Sastuba, J. Toth, and B. Jung, “Converting depth images and
point clouds for feature-based pose estimation,” in 2023
IEEE/RSJ International Conference on Intelligent Robots and
Systems (IROS), 2023.

[50]
A. Dai, A. X. Chang, M. Savva, M. Halber, T. Funkhouser, and M. Nießner,
“Scannet: Richly-annotated 3d reconstructions of indoor scenes,” in
Proceedings of the IEEE conference on computer vision and pattern
recognition, 2017, pp. 5828–5839.

[51]
A. Geiger, P. Lenz, C. Stiller, and R. Urtasun, “Vision meets robotics: The
kitti dataset,” International Journal of Robotics Research (IJRR),

[52]
T. Schops, J. L. Schonberger, S. Galliani, T. Sattler, K. Schindler,
M. Pollefeys, and A. Geiger, “A multi-view stereo benchmark with
high-resolution images and multi-camera videos,” in Proceedings of the
IEEE conference on computer vision and pattern recognition, 2017, pp.

[53]
R. Jensen, A. Dahl, G. Vogiatzis, E. Tola, and H. Aanæs, “Large scale
multi-view stereopsis evaluation,” in Proceedings of the IEEE
conference on computer vision and pattern recognition, 2014, pp. 406–413.

[54]
A. Knapitsch, J. Park, Q.-Y. Zhou, and V. Koltun, “Tanks and temples:
Benchmarking large-scale scene reconstruction,” ACM Transactions on
Graphics, vol. 36, no. 4, 2017.

[55]
J. L. Schonberger and J.-M. Frahm, “Structure-from-motion revisited,” in
Proceedings of the IEEE conference on computer vision and pattern
recognition, 2016, pp. 4104–4113.

[56]
J. L. Schönberger, E. Zheng, J.-M. Frahm, and M. Pollefeys, “Pixelwise
view selection for unstructured multi-view stereo,” in Computer
Vision–ECCV 2016: 14th European Conference, Amsterdam, The Netherlands,
October 11-14, 2016, Proceedings, Part III 14.1em plus 0.5em minus
0.4emSpringer, 2016, pp. 501–518.

[57]
Z. Yang, Z. Ren, Q. Shan, and Q. Huang, “Mvs2d: Efficient multi-view stereo
via attention-driven 2d convolutions,” in Proceedings of the IEEE/CVF
conference on computer vision and pattern recognition, 2022, pp. 8574–8584.

[58]
R. from http://www.cloudcompare.org/, “Cloudcompare (version 2.11),” 2024.

[59]
Y. Ming, X. Yang, W. Wang, Z. Chen, J. Feng, Y. Xing, and G. Zhang,
“Benchmarking neural radiance fields for autonomous robots: An overview,”
arXiv preprint arXiv:2405.05526, 2024.

§ SUPPLEMENTARY

§.§ Good Neighbor Selection

The dense bundle adjustment relies on the selection of good neighbors.
We choose neighbor frames by filtering the relative poses following <ref>.
$[\V G_{nb,1},\cdots],\V G_{i}),\tau_{nb}=2$
select frame by relative pose

$\V G_j\in [\V G_{nb,1},\cdots]$
transformation from i to j
$\V T_{ji}=\V G_i^{-1}\V G_j;$

$\V R_{ji}, \V t_{ji}=\V T_{ji};$

$l_{baseline} = || \V t_{ji}||_2;$

$\theta_{facing} = \arccos([0,0,1]\V R_{ji}[0,0,1]^T);$

$l_{baseline}>\tau_{baseline}$ and $\theta_{facing}>\tau_{facing}$
$Ts.append(\V T_{ji})$

return the best $\tau_{nb}$ XY directional baselines

End Function
Depth neighbor frame selection.

§.§ Description of the Sensor Systems

For collecting our own RGB-X dataset, described in Sec. <ref>, two commercial sensors are employed: the Microsoft Azure Kinect RGB-D sensor and the built-in camera of a Xiaomi Redmi smartphone. Furthermore, two custom-built sensor systems are used for capturing the datasets:

Handheld mapping system
Our custom-built handheld mapping device is show in <ref>(a).
It is based on a commercial camera rig with the sensors mounted to the top bar.
The sensors are a Livox AVIA Lidar and two IDS U3-30C0CP global-shutter cameras with a Sony IMX392 [2.35]MPixel RGB CMOS sensor.
The Lidar sensor has a Field of View (FoV) of $70.4^\circ \times 77.2^\circ$.
The cameras are equipped with [4]mm lenses, which results in a similar FoV of $77.3^\circ \times 61.9^\circ$.
Below the sensors a 3D printed enclosure is mounted with an embedded PC for data recording and power supply electronics.
Cameras and Lidar are co-calibrated using a calibration board.

From the system we extract a synchronized RGB-L data stream with Lidar scans and camera images with [10]Hz.
While the system features stereo cameras, in this work we focus on mono-SLAM.
Therefore, only the camera closest to the Lidar sensor is used.
The data is captured with the system handheld and an average walking speed of [0.84]$\frac{m}{s}$.
During data collection the sensors always points towards the captured object.
The trajectory around Veitshöchheim Palace is [251]m long and was captured in [5]min.
It consists of 3000 Lidar scans and 3000 RGB images.

UAV mapping system
The UAV mapping system is based on the DJI Matrice 300 RTK and is shown in <ref>(b).
It carries an Ouster OS1-128 Lidar and a LUCID Vision Labs Phoenix PHX032S-CC global-shutter camera with a Sony IMX265 [3.2]MPixel RGB CMOS sensor.
The Lidar sensor has a FoV of $360^\circ \times 45^\circ$.
The camera is used with a [4.5]mm fixed lens with a horizontal FoV of $84.7^\circ$.
The co-calibrated camera is mounted directly on top of the Lidar sensor.
For the data collection an embedded PC is mounted on top of the UAV.
The aerial imagery is captured with an oblique angle and a lawnmower pattern flight trajectory.


* [27] F. Golse, B. Perthame, R. Sentis, A compactness result for transport equations and application to the calculation of the limit of the principal eigenvalue of a transport operator, C. R. Acad. Sci. Paris Sér. I Math. 301 (1985), 341–344.
  * [28] L. Grafakos: Classical Fourier Analysis, 2nd edition, Springer, 2008.
  * [29] H. Holden, K.H. Karlsen, D. Mitrović, E.Yu. Panov: Strong Compactness of Approximate Solutions to Degenerate Elliptic-Hyperbolic Equations with Discontinuous Flux Functions, Acta Mathematica Scientia 29B (2009) 1573–1612.
  * [30] K.H. Karlsen and M. Ohlberger, A note on the uniqueness of entropy solutions of nonlinear degenerate parabolic equations, J. Math. Anal. Appl., 275 (2002), 439-–458.
  * [31] M. Lazar, D. Mitrović: Velocity averaging – a general framework, Dynamics of PDEs 9 (2012) 239–260.
  * [32] M. Lazar, D. Mitrović: On an extension of a bilinear functional on $L^{p}(\mathbb{R}^{n})\times E$ to Bôchner spaces with an application to velocity averaging, C. R. Math. Acad. Sci. Paris 351 (2013), 261–264.
  * [33] M. Lazar, D. Mitrović: Existence of solutions for a scalar conservation law with a flux of low regularity, Electronic J. Diff. Eq. 2016 (2016), 1–18.
  * [34] P.-L. Lions, B. Perthame, E. Tadmor: A kinetic formulation of multidimensional scalar conservation law and related equations, J. Amer. Math. Soc. 7 (1994) 169–191.
  * [35] M. Mišur, D. Mitrović: On a generalization of compensated compactness in the $L^{p}-L^{q}$ setting, Journal of Functional Analysis 268 (2015) 1904–1927.
  * [36] W. Neves, E. Panov, and J. Silva, Strong Traces for Conservation Laws with General Nonautonomous Flux, SIAM Journal on Mathematical Analysis 50 (2018), 6049–6081.
  * [37] J. M. Nordbotten, M.A. Celia: Geological Storage of CO2: Modeling Approaches for Large-Scale Simulation, John Wiley and Sons, 2011.
  * [38] J. M. Nordbotten, personal communication.
  * [39] E. Yu. Panov, Existence of strong traces for generalized solutions of multidimensional scalar conservation laws, J. Hyperbolic Differ. Equ. 2 (2005), 885–908.
  * [40] E. Yu. Panov: Ultra-parabolic equations with rough coefficients. Entropy solutions and strong precompactness property, J. Math. Sci. 159 (2009) 180–228.
  * [41] E. Yu. Panov: Ultraparabolic $H$-measures and compensated compactness, Ann. Inst. H. Poincaré Anal. Non Linéaire 28 (2011) 47–62.
  * [42] B. Perthame, P. Souganidis, A limiting case for velocity averaging, Ann. Sci. Ec. Norm. Sup. 4 (1998) 591–598.
  * [43] S. Polidoro, A nonlinear PDE in mathematical finance, In: F.Brezzi, A.Buffa, S.Corsaro, A.Murli (eds), Numerical Mathematics and Advanced Applications, Springer Milano, 2023.
  * [44] F. Rindler: Directional oscillations, concentrations, and compensated compactness via microlocal compactness forms, Arch. Ration. Mech. Anal. 215 (2015) 1–63.
  * [45] E. M. Stein: Singular Integrals and Differentiability Properties of Functions, Princeton University Press, 1970.
  * [46] L. Sylvestre, Oscillation Properties of Scalar Conservation Laws, Comm. Pure App. Math 72 (2019), 1321–1348
  * [47] E. Tadmor, T. Tao: Velocity averaging, kinetic formulations, and regularizing effects in quasi-linear PDEs, Communications on Pure and Applied Mathematics 60 (2007) 1488–1521.
  * [48] L. Tartar: H-measures, a new approach for studying homogenisation, oscillation and concentration effects in PDEs, Proc. Roy. Soc. Edinburgh Sect. A 115 (1990) 193–230.
  * [49] L. Tartar: The General Theory of Homogenization: A Personalized Quest, Springer-Verlag Berlin Heidelberg, 2009.
  * [50] L. Tartar: Multi-scale H-measures, Discrete and Continuous Dynamical Systems S 8 (2015) 77–90.
  * [51] H. Triebel, Interpolation Theory, Function Spaces, Differential Operators, NorthHolland Mathematical Library 18. North-Holland, Amsterdam. (1978)
  * [52] A. I. Vol’pert, S. I. Hudjaev: Cauchy’s problem for degenerate second order quasilinear parabolic equations, Math. USSR Sbornik 7 (1969) 365–387.
  * [53] Z. Wu and J. Yin, Some properties of functions in BV and their applications to the uniqueness of solutions for degenerate quasilinear parabolic equations, Northeastern Math. J., 5 (1989), 395–422.


# Ant Colony Optimization for Cooperative Inspection Path Planning Using
Multiple Unmanned Aerial Vehicles

Duy Nam Bui1, Thuy Ngan Duong2, Manh Duong Phung3 1Duy Nam Bui is with the
Vietnam National University, Hanoi, Vietnam. e-mail<EMAIL_ADDRESS>Ngan
Duong is with the Ulsan National Institute of Science and Technology, Ulsan,
Korea. e-mail<EMAIL_ADDRESS>Duong Phung is with the Fulbright
University Vietnam, Ho Chi Minh City, Vietnam. e-mail:
<EMAIL_ADDRESS>
###### Abstract

This paper presents a new swarm intelligence-based approach to deal with the
cooperative path planning problem of unmanned aerial vehicles (UAVs), which is
essential for the automatic inspection of infrastructure. The approach uses a
3D model of the structure to generate viewpoints for the UAVs. The calculation
of the viewpoints considers the constraints related to the UAV formation
model, camera parameters, and requirements for data post-processing. The
viewpoints are then used as input to formulate the path planning as an
extended traveling salesman problem and the definition of a new cost function.
Ant colony optimization is finally used to solve the problem to yield optimal
inspection paths. Experiments with 3D models of real structures have been
conducted to evaluate the performance of the proposed approach. The results
show that our system is not only capable of generating feasible inspection
paths for UAVs but also reducing the path length by 29.47% for complex
structures when compared with another heuristic approach. The source code of
the algorithm can be found at https://github.com/duynamrcv/aco_3d_ipp.

###### Index Terms:

Coverage path planning, infrastructure monitoring, vision-based inspection,
ant colony optimization, unmanned aerial vehicle

##  I Introduction

Unmanned aerial vehicles (UAVs) with their high flexibility and versatile
deployment play an important role in structural health monitoring (SHM) to
carry out non-destructive inspection, evaluation of cracks or damages, and
prediction of catastrophic failures [1, 2, 3]. One of the essential tasks in
SHM is solving the inspection path planning (IPP) problem as it generates the
path the UAVs need to follow to collect information about the structure [1,
3]. However, there are few IPP methods for aerial robotics, particularly when
it comes to collaborative inspection (C-IPP). The C-IPP involves multiple UAVs
cooperating in a formation to enhance inspection solutions, such as decreasing
the total inspection time [4, 5].

In the literature, most works address the inspection problem in 2D space [4,
1]. For 3D structures, most studies deal with it by using a visual approach
that relies on image sensors equipped on UAVs [1, 6, 5]. In [5], 3D visual
inspection trajectories for UAVs are generated based on artificial potential
field to inspect complex structures. In [7], a heuristic method that splits
the inspecting structure into multiple layers is used to generate paths for a
group of UAVs to carry out the visual inspection. While these methods create
feasible flight paths for UAVs, the optimality of those paths is hardly
addressed.

In another direction, the growing use of artificial intelligence is
transforming the way the operation and management of SHM tasks are conducted
[8]. The major topics that make significant contributions include expert
systems, fuzzy logic, machine learning, and swarm intelligence. In particular,
the application of swarm intelligence (SI) algorithms in the SHM problem is
being widely studied [1, 9, 10]. SI algorithms are inspired by the collective
behaviors of decentralized, self-organizing, natural, or man-made systems. The
popular algorithms that can be referred to include the genetic algorithm (GA),
particle swarm optimization (PSO) and ant colony optimization (ACO). In [9], a
modified genetic algorithm was developed to inspect large-scale, complex 3D
structures by using multiple UAVs. In [10], PSO is used to address the co-
optimal coverage path planning problem, where it concurrently optimizes the
path of the UAV, the quality of the captured images, and the computational
complexity. The nearest neighbor combined with GA is used in [11] to optimize
the coverage paths for multiple UAVs to reconstruct the 3D model of post-
disaster damaged buildings. However, most studies only consider solving the
inspection path for individual UAVs or discretizing the inspection space to
simplify the IPP problem. The C-IPP for multiple UAVs is therefore still a
challenging problem that needs further investigation.

This paper presents a new approach to the C-IPP problem for multiple UAVs to
carry out inspection tasks. The approach utilizes an effective sampling-based
method to calculate the viewpoint positions. The overall problem is then
modeled as an extended traveling salesman problem (TSP) and solved by the ACO
algorithm to obtain the optimal inspection path for a given structure. The
main contributions of our work are threefold: (i) model the UAV formation
based on the camera carried on each individual UAV and then propose a method
to generate viewpoints for visual data collection; (ii) formulate the
inspection path planning as an extended TSP problem with a new fitness
function and solve it using the ACO algorithm; (iii) experiment with real-
world 3D models to benchmark the performance of the proposed method in
comparison with other algorithms.

##  II System overview

The problem of collaborative inspection path planning (C-IPP) is finding
efficient paths that guide autonomous systems such as unmanned aerial vehicles
to inspect the region of interest with onboard sensors, e.g., camera sensors.
In this paper, the use of swarm intelligence is investigated to perform the
C-IPP for a group of UAVs to carry out the inspection task. The inspecting
structure such as a building or a bridge is known, e.g., by using building
information modeling (BIM) or 3D lidar scanners [12, 13]. The inspection path
for the UAV formation is defined as sequentially connected waypoints that
indicate the location of the UAV formation. Path efficiency is measured in
path length. The path also needs to satisfy constraints related to the
sensor’s field of view, the distance from the UAVs to the inspecting surface,
and requirements for image stitching.

Figure 1: Overview of the proposed method

Our approach to the C-IPP problem is presented in Fig.1. The system first
takes 3D models of the structure such as the STL or PLY files as the input. It
then pre-processes them to sample down and filter noises before passing
through the viewpoints generation module. This module calculates the points
that the UAVs need to fly through to collect visual data of the structure. The
viewpoints are then fed to the path optimization module that uses the ACO to
generate inspection paths.

##  III Generation of viewpoints for UAV formation

To collect visual information of the structure’s surface, it is necessary to
determine the viewpoints at which the UAVs take photos. The calculation of
those viewpoints depends on the UAV formation, the camera parameters, and the
requirements for post-processing with details as follows.

###  III-A Visual-based UAV formation model

Figure 2: The camera’s field of view

The formation model used in this work is the virtual leader-follower [14]. In
this model, the leader is a non-physical UAV used as a reference for other
UAVs, the followers, to determine their location. Each follower carries an RGB
camera that can be stabilized and controlled via a three-axis gimbal. The
gimbal controls the camera so that it is perpendicular to the inspecting
surface. The footprint of the camera on the inspecting surface is illustrated
as shown in Fig. 2. Let $d$ be the working distance from the UAV to the
structure’s surface so that the capturing photos meet the resolution required
for the inspection task [1]. Height $h$ and width $w$ of the camera’s
footprint are then given by:

$\displaystyle h$ $\displaystyle=2d\tan\dfrac{\alpha}{2}$ (1) $\displaystyle
w$ $\displaystyle=2d\tan\dfrac{\beta}{2},$

where $\alpha$ and $\beta$ are the horizontal and vertical angles of the
camera’s field of view (FOV), respectively. The overall footprint is then the
union of the individual footprints but includes overlapping areas for image
stitching. This area is given by $o_{ij}=h_{ij}\times w_{ij}$, $0<h_{ij}\leq
h$, $0<w_{ij}\leq w$, where $h_{ij}$ and $w_{ij}$ respectively be the width
and height of the overlapping area between the footprints of UAV $i$ and $j$.

Figure 3: Overlapping area of two footprints

Without loss of generality, the camera points along the $x-$axis of the body
frame and the local frame attached to each footprint is defined as shown in
Fig. 3, where $B_{i}$ is the center of footprint $i$. The distances of between
UAVs $i$ and $j$, $B_{i}B_{j}$, along the $y$ and $z$ axes are given by
$(w-w_{ij})$ and $(h-h_{ij})$, respectively. Denote $\gamma_{ij}$ as the angle
formed by $B_{i}B_{j}$ and $B_{iz}$, and
$\delta_{ij}=\overrightarrow{B_{i}B_{j}}$ as the vector representing the
difference in position between UAVs $i$ and $j$. This difference is computed
as follows:

$\delta_{ij}=\overrightarrow{B_{i}B_{j}}=\left[\begin{array}[]{c}0\\\
\left(w-w_{ij}\right)\text{sgn}\left(\sin\gamma_{ij}\right)\\\
\left(h-h_{ij}\right)\text{sgn}\left(\cos\gamma_{ij}\right)\end{array}\right],$
(2)

where $\text{sgn}\left(\cdot\right)$ denotes the sign function. Let
$\Delta_{i}$ and $\Delta_{j}$ be the vectors from the virtual leader to UAV
$i$ and UAV $j$, respectively. $\Delta_{j}$ is then computed as:

$\Delta_{j}=\Delta_{i}+\delta_{ij}.$ (3)

From (2) and (3), vectors $\Delta_{i},\forall i\in\left\\{1,...,n\right\\}$,
can be computed so that the corresponding position of each UAV satisfies the
target formation. The largest width, $w_{f}$, and height, $h_{f}$, of the
formation footprint are then obtained to calculate viewpoints.

###  III-B Viewpoint generation

Figure 4: Horizontal and vertical overlapping requirements Figure 5: Virtual
leader’s viewpoint generation

Given the overall footprint of the formation, the viewpoints at which the UAVs
take photos of the surface can be determined as illustrated in Fig. 4. The
required distances between two consecutive points in the horizontal direction,
$\delta_{w}$, and vertical direction, $\delta_{h}$, are computed based on the
overlapping width, $w_{o}$, and height, $h_{o}$, as:

$\displaystyle\delta_{w}=w_{f}-w_{o},$ (4)
$\displaystyle\delta_{h}=h_{f}-h_{o}.$

The 3D model of the structure, denoted as $S$, is then sliced by multiple
horizontal planes, defined as $\lambda_{j}$, as shown in Fig. 5. The value of
$\lambda_{j}$ ranges from $(\min_{z}S+\dfrac{h_{f}}{2})$ to
$(\max_{z}S-\dfrac{h_{f}}{2})$ along the $z-$axis with step $\delta_{h}$. The
intersection between the 3D model and plane $\lambda_{j}$ is computed as

$C_{j}=\left\\{\left(x,y,z\right)\in\mathbb{R}^{3}:\left\langle\overrightarrow{v},S\left(x,y,z\right)\right\rangle-\lambda_{j}=0\right\\},$
(5)

where $C_{j}\left(x,y,z\right)$ are the points of the intersecting surface,
$\overrightarrow{v}=\left[0,0,1\right]^{T}$ is the normal vector of the plane,
and $\left\langle\cdot,\cdot\right\rangle$ is the inner product operation [7].
For complex structures, $C_{j}$ may include more than one cluster of points,
and thus should be processed by clustering algorithms such as DBSCAN [15], to
obtain clusters $C_{j}^{k}(x,y,z)$.

Let $d$ be the distance from the UAV formation to the inspecting surface. A
viewpoint $P_{j}^{k}$ is then obtained by extending the intersection set by
$d$ as:

$P_{j}^{k}\left(x,y,z\right)=C_{j}^{k}\left(x,y,z\right)\pm
d\overrightarrow{v}_{j},$ (6)

where $\overrightarrow{v}_{j}$ is the normal vector of the inspecting surface
pointing through $C_{j}^{k}(x,y,z)$ and $\pm$ indicates the direction of the
normal vector [16]. The set of all viewpoints, $P$, is then obtained by
merging all $P_{j}^{k}$ and down-sampling them by distance $\delta_{w}$.

##  IV Ant colony optimization for inspection path planning

This section presents our formulation and implementation of the ACO algorithm
to generate inspection paths.

###  IV-A Inspection path planning problem

After determining the set of viewpoints $P$, it is necessary to plan a path
through these points that the UAVs can follow. The path should be optimal and
meet the constraints related to UAV operation. Since the viewpoints are
generated for the formation, the planning path will be assigned to the virtual
leader. Our approach is modeling this problem as an extended TSP problem,
where the goal is to find a Hamiltonian path from the set of viewpoints. The
TSP problem is formulated as a graph $G=(P,E,F)$, where
$P=\\{P_{1},P_{2},...,P_{m}\\}$ is the set of viewpoints, $E=\\{(i,j)|(j,i)\in
p\times p\\}$ is the set of edges connecting every two vertices with cost
value $F_{ij}$ [17]. The objective is to minimize the cost of the travel tour,
$J$, which is described as follows:

$J=\sum_{i=1}^{m}\left(\sum_{j\neq i,j=1}^{m}F_{ij}\chi_{ij}\right),$ (7)

where $\chi_{ij}=\\{0,1\\}$ represents whether there is a direct edge
connecting two nodes $i$ and $j$.

$F_{ij}$ is the traveled cost from point $i$ to $j$ and is evaluated via a
sequential convex function as follows:

$F_{ij}=w_{1}\left\|\overrightarrow{P_{i}^{xy}P_{j}^{xy}}\right\|+w_{2}\left\|\overrightarrow{P_{i}^{z}P_{j}^{z}}\right\|,$
(8)

where $P_{i}=\left[P_{i}^{x},P_{i}^{y},P_{i}^{z}\right]^{T}$ denotes the
position of the viewpoints in 3D space, $P_{i}^{xy}$ denotes the $x$ and $y$
components of $P_{i}$ and $P_{i}^{z}$ denotes the $z$ component; $w_{1}$ and
$w_{2}$ are positive weights. We select $w_{2}>w_{1}$ to punish more the
difference in the $z$-axis since it affects the altitude of the UAV formation.

###  IV-B Optimal inspection path planning using Ant Colony Optimization

The extended TSP problem formulated in (7) - (8) is NP-hard that cannot be
solved by analytical methods. Instead, we propose to use a metaheuristic
optimization technique called the ACO to take advantage of swarm intelligence
in solving complex combinatorial optimization problems [18]. This algorithm is
inspired by the ants foraging in which ants lay chemical pheromone trails to
discover efficient paths between their nest and food supplies. The pheromone
$\tau_{ij}$ contributed by $l$ ants to edge $E_{ij}$ is computed as follows:

$\tau_{ij}=(1-\varrho)\tau_{ij}+\sum_{k=1}^{l}\Delta\tau_{ij}^{k},$ (9)

where $\varrho$ is the pheromone evaporation rate, and $\Delta\tau_{ij}^{k}$
is the pheromone quantity that ant $k$ leaves on edge $E_{ij}$. It is given
by:

$\Delta\tau_{ij}^{k}=\chi_{ij}\dfrac{Q}{F_{ij}},$ (10)

where $Q$ is a positive constant. The probability of an ant $k$ moving from
state $i$ to state $j$ is computed as:

$\rho_{ij}^{k}=\dfrac{\left(\tau_{ij}\right)^{a}\left(\eta_{ij}\right)^{b}}{\sum\limits_{z\in\Omega}\left(\tau_{iz}\right)^{a}\left(\eta_{iz}\right)^{b}},$
(11)

where $\Omega$ is the set of unvisited viewpoints of ant $k$ at state $i$, $a$
and $b$ are the parameters representing the influence of pheromones and
heuristic information, and $\eta_{ij}=(F_{ij})^{-1}$ is the heuristic
information.

/* Initialization */

1 Get the list of $m$ viewpoints ;

2 Initialize parameters $l$, $a$, $b$, $Q$, $\varrho$;

3 Initialize pheromone $\tau(0)$ and calculate $\eta$ matrix;

4

/* Searching */

5 while  _iter $\leq$ max_iteration_ do

6 for  _Ant $k$ in colony_ do

7 for  _i $\leftarrow 1:(m-1)$ _ do

Choose the next viewpoint $j$ with the probability $\rho^{k}_{ij}$;

/* Equation 11 */

8

Evaluate $J_{k}$ of tour given by ant $k$;

/* Equation 7 */

9

Update pheromone quantity $\tau_{ij}$ in each edge;

/* Equation 9, 10 */

10 Store the best solution;

11

12Return the best solution as the inspection path;

Algorithm 1 Pseudocode for the ACO algorithm

The implementation of the ACO algorithm to solve the extended TSP problem for
3D inspection path planning is described in Algorithm 1. Here, each viewpoint
is considered as a node of the path and the line segment connecting two nodes
is represented as an edge. Initially, the amount of pheromone $\tau_{ij}$ in
each edge is set equally. Each time, an ant in the colony chooses its route
based on probability $\rho^{k}$ computed in (11) and then calculates the
fitness of that path using (7). After traveling through an edge $E_{ij}$, each
ant $k$ leaves an amount of pheromone trail $\Delta\tau_{ij}^{k}$ on that edge
inversely proportional to the value $F_{ij}$ of the edge. The total quantity
of pheromones $\tau_{ij}$ in each edge is then updated based on the visited
ants and the evaporation amount as in (9) and (10). From there, the
probability matrix $\rho^{k}$ is updated to guide the next ants to choose
better routes. The procedure is reiterated until it reaches the maximum
allowable number of iterations. The path having the smallest value of $J$ is
chosen as the inspection path and is assigned to the virtual leader of the UAV
formation. The real follower UAVs then use that path as a reference to adjust
their position according to (3) to form the expected formation for data
collection.

##  V Results

Figure 6: The inspection formation structure

(a) 3D model

(b) Generated viewpoints

(c) Inspection path

Figure 7: Scenario 1 - The Turtle Tower

(a) 3D model

(b) Generated viewpoints

(c) Inspection path

Figure 8: Scenario 2 - The twin tower

(a) Scenario 1

(b) Scenario2

Figure 9: Best fitness values over iterations of the ACO algorithm

In this section, simulation experiments are conducted to evaluate the
performance of the proposed algorithm.

###  V-A Scenario setup

The UAVs used in this work are 3DR Solo drones that can be programmed to fly
automatically via ground control station software. Each UAV is equipped with
an IMX-100 camera for data collection, with the angles of view
$\alpha=49.4^{0}$ and $\beta=63^{0}$. In the evaluation, we used 4 UAVs for
the inspection, whose formation structure is depicted as Fig. 6. The distance
between the infrastructure surface and inspection system is chosen as $d=20$ m
[6]. From (1) and the image stitching requirement, the overall footprint of
the formation is obtained as $w_{f}=48$ m and $h_{f}=34$ m.

Experiments have been carried out on two scaled real structure models
described by STL data as shown in Fig. 7(a) and Fig. 8(a). The first model is
the tower named Turtle Tower with a size of 150 m $\times$ 210 m $\times$ 231
m. The second on is a twin tower with a size of 119.14 m $\times$ 242.61 m
$\times$ 669.75 m. The second structure is challenging to inspect as it
includes two blocks connected by a bridge in the middle. In experiments,
parameters of the ACO algorithm are chosen as $\alpha=\beta=1$,
$\varrho=0.05$, $Q=1$, $w_{1}=1$, $w_{2}=2$. The number of ants is set to 100.
The maximum iteration is set to 500.

###  V-B Path generation and IPP convergence

Figures 7(b) and 8(b) show the viewpoints generated to inspect the two
structures. It can be seen that the viewpoints are well spread throughout the
structures to cover all surfaces. Figures 7(c) and 8(c) show the generated
inspection paths. For the first structure, the path mainly goes along each
layer from the bottom to the top similar to the simple back-and-forth
strategy. The path for the second structure, however, is more complicated
since the simple heuristic path generation strategies would not yield good
results. The fitness values of the inspection paths during the optimization
process are described in Fig 9. It can be seen that they converge to small
values in both scenarios and thus show the efficiency of the ACO algorithm in
providing optimal solutions to the C-IPP problem.

TABLE I: Fitness values of the paths generated by the BAF and ACO Scenario |  BAF | ACO |  Improvement
---|---|---|---
|  |  Mean |  Std |
1 |  1.2297e+04 |  1.1173e+04 |  54 |  9.14%
2 |  8.1639e+04 |  5.7582e+04 |  142 |  29.47%

To further evaluate the performance of the proposed approach, a comparison
with the back-and-forth (BAF) algorithm [19] is conducted in which the cost
values of the inspection path generated by the BAF algorithm are obtained via
the same fitness function designed in (7). Table I shows the comparison
results. It can be seen that for the first structure, the improvement of our
algorithm is not significant due to the simplicity of the structure. For the
second structure, however, the improvement of 29.47% clearly shows the
superior performance of our algorithm compared to the BAF. It proves that the
proposed method is capable of providing efficient inspection paths for 3D
structures with different complexity.

##  VI Conclusion

In this work, we have presented a new approach for the cooperative path
planning problem to inspect 3D structures using a group of UAVs. The approach
combines our calculation of the viewpoints for UAV formation to collect visual
data of the structure and an optimal path planning method using nature-
inspired intelligence named ACO. The evaluation results show that the proposed
approach is superior to other heuristic algorithms in generating inspection
paths for complex 3D structures. The results thus confirm the efficiency and
applicability of the proposed method for real-world scenarios.

## Acknowledgement

Duy Nam Bui was funded by the Master, PhD Scholarship Programme of Vingroup
Innovation Foundation (VINIF), code VINIF.2022.Ths.057.

## References

  * [1] M. D. Phung, C. H. Quach, T. H. Dinh, and Q. Ha, “Enhanced discrete particle swarm optimization path planning for UAV vision-based surface inspection,” _Automation in Construction_ , vol. 81, pp. 25–33, 2017.
  * [2] V. Hoang, M. Phung, T. Dinh, and Q. Ha, “Angle-encoded swarm optimization for UAV formation path planning,” in _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , Oct 2018, pp. 5239–5244.
  * [3] Y. Wang, Y. Li, F. Yin, W. Wang, H. Sun, J. Li, and K. Zhang, “An intelligent UAV path planning optimization method for monitoring the risk of unattended offshore oil platforms,” _Process Safety and Environmental Protection_ , vol. 160, pp. 13–24, 2022.
  * [4] Y. Wu, S. Wu, and X. Hu, “Multi-constrained cooperative path planning of multiple drones for persistent surveillance in urban environments,” _Complex & Intelligent Systems_, vol. 7, no. 3, pp. 1633–1647, Jun 2021\.
  * [5] S. Ivić, B. Crnković, L. Grbčić, and L. Matleković, “Multi-UAV trajectory planning for 3D visual inspection of complex structures,” _Automation in Construction_ , vol. 147, p. 104709, 2023.
  * [6] G. Li, S. He, Y. Ju, and K. Du, “Long-distance precision inspection method for bridge cracks with image processing,” _Automation in Construction_ , vol. 41, pp. 83–95, 2014.
  * [7] S. S. Mansouri, C. Kanellakis, E. Fresk, D. Kominiak, and G. Nikolakopoulos, “Cooperative coverage path planning for visual inspection,” _Control Engineering Practice_ , vol. 74, pp. 118–131, 2018.
  * [8] R. Panchalingam and K. C. Chan, “A state-of-the-art review on artificial intelligence for smart buildings,” _Intelligent Buildings International_ , vol. 13, no. 4, pp. 203–226, 2021.
  * [9] W. Jing, D. Deng, Y. Wu, and K. Shimada, “Multi-UAV coverage path planning for the inspection of large and complex structures,” in _2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , Oct 2020, pp. 1480–1486.
  * [10] Z. Shang, J. Bradley, and Z. Shen, “A co-optimal coverage path planning method for aerial scanning of complex structures,” _Expert Systems with Applications_ , vol. 158, p. 113535, 2020.
  * [11] R. Nagasawa, E. Mas, L. Moya, and S. Koshimura, “Model-based analysis of multi-UAV path planning for surveying postdisaster building damage,” _Scientific Reports_ , vol. 11, no. 1, p. 18588, Sep 2021.
  * [12] H. Son, F. Bosché, and C. Kim, “As-built data acquisition and its use in production monitoring and automated layout of civil infrastructure: A survey,” _Advanced Engineering Informatics_ , vol. 29, no. 2, pp. 172–183, 2015, infrastructure Computer Vision.
  * [13] N. Bolourian and A. Hammad, “Lidar-equipped UAV path planning considering potential locations of defects for bridge inspection,” _Automation in Construction_ , vol. 117, p. 103250, 2020.
  * [14] L. V. Santana, A. S. Brandão, and M. Sarcinelli-Filho, “On the design of outdoor leader-follower UAV-formation controllers from a practical point of view,” _IEEE Access_ , vol. 9, pp. 107 493–107 501, 2021.
  * [15] M. Ester, H.-P. Kriegel, J. Sander, and X. Xu, “A density-based algorithm for discovering clusters in large spatial databases with noise,” in _Proceedings of the Second International Conference on Knowledge Discovery and Data Mining_ , ser. KDD’96. AAAI Press, 1996, p. 226–231.
  * [16] X.-Z. Liu, J.-H. Yong, G.-Q. Zheng, and J.-G. Sun, “An offset algorithm for polyline curves,” _Computers in Industry_ , vol. 58, no. 3, pp. 240–254, 2007.
  * [17] K. Chaudhari and A. Thakkar, “Travelling salesman problem: An empirical comparison between ACO, PSO, ABC, FA and GA,” in _Emerging Research in Computing, Information, Communication and Applications_ , N. R. Shetty, L. M. Patnaik, H. C. Nagaraj, P. N. Hamsavath, and N. Nalini, Eds. Singapore: Springer Singapore, 2019, pp. 397–405.
  * [18] M. Dorigo, M. Birattari, and T. Stutzle, “Ant colony optimization,” _IEEE Computational Intelligence Magazine_ , vol. 1, no. 4, pp. 28–39, Nov 2006.
  * [19] E. V. Vazquez-Carmona, J. I. Vasquez-Gomez, J. C. Herrera-Lozada, and M. Antonio-Cruz, “Coverage path planning for spraying drones,” _Computers & Industrial Engineering_, vol. 168, p. 108125, 2022.