R³LIVE++: A Robust, Real-Time, Radiance Reconstruction Package With a Tightly-Coupled LiDAR-Inertial-Visual State Estimator

Jiarong Lin , Member, IEEE, and Fu Zhang , Member, IEEE

Abstract—This work proposed a LiDAR-inertial-visual fusion framework termed R³LIVE++ to achieve robust and accurate state estimation while simultaneously reconstructing the radiance map on the fly. R³LIVE++ consists of a LiDAR-inertial odometry (LIO) and a visual-inertial odometry (VIO), both running in real-time. The LIO subsystem utilizes the measurements from a LiDAR for reconstructing the geometric structure, while the VIO subsystem simultaneously recovers the radiance information of the geometric structure from the input images. R³LIVE++ is developed based on R³LIVE and further improves the accuracy in localization and mapping by accounting for the camera photometric calibration and the online estimation of camera exposure time. We conduct more extensive experiments on public and self-collected datasets to compare our proposed system against other state-of-the-art SLAM systems. Quantitative and qualitative results show that R³LIVE++ has significant improvements over others in both accuracy and robustness. Moreover, to demonstrate the extendability of R³LIVE++, we developed several applications based on our reconstructed maps, such as high dynamic range (HDR) imaging, virtual environment exploration, and 3D video gaming.

Index Terms—3D reconstruction, SLAM, sensor fusion, state estimation.

I. INTRODUCTION

IMULTANEOUS localization and mapping (SLAM) is a technology that utilizes a sequence of sensor (e.g., camera, LiDAR, IMU, etc.) data to estimate the sensor poses and simultaneously reconstruct the 3D map of surrounding environments. Since SLAM can estimate poses in real-time, it has been widely applied in localization and feedback control for autonomous robots (e.g., unmanned aerial vehicles [1], [2], automated ground vehicles [3], [4], [5], and self-driving cars [6], [7], [8]). Meanwhile, with the capacity to reconstruct the map in real-time, SLAM is also crucial in various robot navigation, virtual and augmented reality (VR/AR), surveying, and mapping applications. Different applications usually require a different level of mapping details: sparse feature map, 3D dense point cloud map, and 3D radiance map (i.e., a 3D point cloud map with radiance information). For example, the sparse visual feature map is suitable and has been widely used for camera-based localization, where the sparse features observed in images can be used for calculating the camera’s pose [9], [10]. The 3D dense point cloud can capture the geometrical structure of the environment even for tiny objects. Hence it is widely used in robot navigation and obstacle avoidance [2], [11]. Finally, radiance maps containing both geometry and radiance information are used in mobile mapping, AR/VR, video gaming, 3D simulation, and surveying. These applications require both geometric structures and textures to provide virtual environments alike the real world [12], [13].

Existing SLAM systems can be mainly categorized into two classes based on the used sensor: visual SLAM and LiDAR SLAM. Visual SLAM is based on low-cost and SWaP (size, weight, and power)-efficient camera sensors and has achieved satisfactory results in localization accuracy. The rich colorful information measured by cameras also makes the reconstructed map suitable for human interpretation. However, due to the lack of direct accurate depth measurements, the mapping accuracy and resolution of visual SLAM are usually lower than LiDAR SLAM. To be more specific, visual SLAM maps the environments by triangulating disparities from multi-view images (e.g., structure from motion for mono-camera, stereo-vision for stereo-camera), an extremely computationally expensive process that often requires hardware acceleration or server clusters. Moreover, limited by the measurement noises and the baseline of multi-view images, the computed depth accuracy drops quadratically with the measurement distance, making visual SLAM difficult to reconstruct large-scale outdoor scenes. Furthermore, visual SLAM can only work in scenarios with good illuminations and will degenerate in high-occlusion or texture-less environments.

On the other hand, LiDAR SLAM is based on LiDAR sensors. Benefiting from the high measurement accuracy (a few centimeters) and the long measurement range (hundreds ofmeters) ofLi-DAR sensors, LiDAR SLAM can achieve much higher accuracy and efficiency on both localization and map reconstruction than visual SLAM. However, LiDAR SLAM easily fails in scenarios with insufficient geometry features, such as in long tunnel-like corridors, facing a single big wall, etc. Moreover, LiDAR SLAM can only reconstruct the geometric structure of the environment, but lacks color information.

Fig. 1. The radiance map of HKU (a and b) and HKUST campuses (c) reconstructed by R³LIVE++ in real-time (see our accompanying video on YouTube: https://youtu.be/kZ8_7k3HpIk).

Fusing both LiDAR and camera measurements in the SLAM could overcome the degeneration issues of each sensor in localization and produce an accurate, textured, and high-resolution 3D map that suffices the needs of various mapping applications. Motivated by this, we propose R³LIVE++, which has the following features:

- It is a LiDAR-Inertial-Visual fusion framework that tightly couples two subsystems: the LiDAR-inertial odometry (LIO) subsystem and the visual-inertial odometry (VIO) subsystem. The two subsystems jointly and incrementally build a 3D radiance map of the environment in real-time. In particular, the LIO subsystem reconstructs the geometric structure by registering new points in each LiDAR scan to the map, and the VIO subsystem recovers the radiance information by rendering pixel colors in each image to points in the map.

- It has a novel VIO design, which tracks the camera pose (and estimates other system states) by minimizing the radiance difference between points from the radiance map and a sparse set of pixels in the current image. The frame-to-map alignment effectively lowers the odometry drift, and the direct photometric error on a sparse set of individual pixels effectively constrains the computation load. Moreover, based on the photometric errors, the VIO is able to estimate the camera exposure time online, which enables the recovery of environment’s true radiance information.

- It is extensively validated in real-world experiments in terms of localization accuracy, robustness, and radiance map reconstruction accuracy. Benchmark results on 25 sequences from an open dataset (the NCLT-dataset) show that R³LIVE++ achieves the highest overall accuracy among all other state-of-the-art SLAM systems (e.g., LVI-SAM, LIO-SAM, FAST-LIO2, etc.) under comparison. The evaluations on R3LIVE-dataset (self-collected) show that our proposed framework is robust to extremely challenging scenarios that LiDAR and/or camera measurements degenerate (e.g., when the device is facing a single texture-less wall). Finally, compared with other counterparts, R³LIVE++ estimates the camera exposure time more accurately and reconstructs the true radiance information of the environment with significantly smaller errors when - It is, to our best knowledge, the first radiance map reconstruction framework that can achieve real-time performance on a PC equipped with a standard CPU without any hardware or GPU accelerations. The system is completely open-sourced to ease the reproduction of this work and benefit the follow-up research. Based on a set of offline utilities for mesh reconstruction and texturing further developed, the system shows high potential in a variety of real-world applications, such as 3D HDR imaging, physics simulation, and video gaming.

II. RELATED WORKS

In this chapter, we review existing works related to our method or system, including LiDAR SLAM, visual SLAM, and LiDARvisual fused SLAM. Due to the large number of existing works, any attempts to give a full review would be incomplete. Hence, we only select the most relevant ones of each branch for review. For a comprehensive review of the SLAM literature, please refer to the work in [14].

A. LiDAR(-Inertial) Odometry

In recent years, the rapid development of LiDAR technologies has greatly improved the reliability and performance of LiDAR sensors while significantly reducing the cost. This progress has drawn increasing research attention to LiDAR SLAM [15]. Zhang et al. proposed a real-time LiDAR odometry and mapping framework, LOAM [16], which achieved localization through scan-to-scan point registration and mapping through scan-tomap registration. In both registrations, only edge and plane feature points were considered to lower the computation load. To enable the algorithm to run in real-time on computation-limited platforms, Shan et al. [17] proposed a lightweight and groundoptimized LOAM (LeGO-LOAM), which discarded unreliable features in the step of ground plane segmentation. Chanoh et al. in [18] presented a novel map-centric SLAM framework for improving map quality for 3D LiDAR SLAM, which is achieved with a continuous-time trajectory representation and novel loop closure methods. These works [16], [17], [18] were mainly based on multi-line spinning LiDARs. For emerging solid-state LiDARs with irregular scanning and very small FoV, our previous works [15] used direct scan-to-map registration to achieve localization and mapping.

To further improve the accuracy and robustness of LiDAR odometry, many frameworks that fused LiDAR measurements with inertial sensors were proposed. In LOAM [16], an IMU could be used to de-skew the LiDAR scan and provide a motion prior for the scan-to-scan registration. It was a looselycoupled method since the IMU bias (and the full state vector) was not involved in the scan registration process. Compared with loosely-coupled methods, tightly-coupled methods showed higher robustness and accuracy, thus drawn increasing research interest recently. Authors in [19] proposed LIOM, which used a graph optimization based on priors from LiDAR-Inertial odometry and a rotation-constrained refinement method. Compared with the former algorithms, LIO-SAM [20] optimized a sliding window of keyframe poses in a factor graph to achieve higher accuracy. Similarly, Li et al. proposed LiLi-OM [21] for both multi-line and solid-state LiDARs based on a sliding window optimization technique. LINS [22] was the first tightly-coupled LIO that solved the 6 DOF ego-motion via iterated Kalman filtering. To lower the high computation load in calculating the Kalman gain, FAST-LIO [23] proposed a new formula for the Kalman gain computation. The resultant computation complexity depended on the state dimension instead of the measurement dimension. Its successor FAST-LIO2 [24] further improved the computation efficiency by proposing an incremental k-d tree. Such a data structure could significantly reduce the time cost of nearest points search and allow the registration of raw points (instead of feature points, such as planes and edges, in past works). The method using raw points was termed as a ”direct” method and could exploit subtle features in the environment, thus increasing the localization accuracy and robustness.

The LIO subsystem of $R^{3} L I V E + +$ is largely based on FAST-LIO2 [24] since it achieves the best overall performance among its counterparts in terms of accuracy, efficiency, and robustness. Moreover, to address the LiDAR degeneration problem and further improve the localization accuracy, we fuse the LIO subsystem with our VIO subsystem in a tightly-coupled manner.

B. Visual(-Inertial) Odometry

Depending on how a camera measurement is formulated, we review the works of visual odometry by categorizing them into two branches based on the criteria proposed in [25]: indirect and direct. These two types of methods have very different pipelines: the former one (indirect method) includes feature extraction, data association, and minimization of feature re-projection error. In contrast, the latter one (direct method) directly minimizes the photometric error (or intensity discrepancy) between consecutive images.

Indirect visual odometry is also called the feature-based method, which had a quite long history. MonoSLAM [26] proposed by Davison et al. was the first monocular visual odometry, which recovered the 3D trajectory of a camera in real-time by creating a sparse but persistent map of natural landmarks within a probabilistic framework. PTAM [27] proposed by Klein and Murray split the tracking and mapping in parallel threads. Visual landmarks in the map were selected from only a few frames to allow efficient bundle-adjustment (BA) optimization that estimated the camera pose and landmark position. Following this idea, a more complete and reliable framework ORB-SLAM [28] was proposed. ORB-SLAM utilized the same feature (i.e., ORB feature) for all the involved tasks, including tracking, mapping, relocalization, and loop closing. Its further work ORB-SLAM2 [29] improved the accuracy by utilizing the metric scale provided by stereo or RGB-D cameras. The scale issue in pure visual SLAM could also be addressed by fusing inertial sensor data, as demonstrated by VINS-Mono [30], Kimera [31], and ORB-SLAM3 [32], which achieve high-accuracy localization by fusing IMU measurements and image features in a sliding window bundle adjustment optimization.

Direct visual odometry is also called photometric-based method, which minimized the intensity differences rather than a geometric error. It was successfully applied in 2D sparse feature tracking (e.g., Lucas–Kanade optical flow [33]) and then extended to visual odometry. LSD-SLAM [34], proposed by Engel et al. was a direct monocular odometry algorithm with both tracking and mapping directly operating on image intensities. It incrementally tracked the camera pose using direct image alignment and simultaneously performed a pose graph optimization to keep the entire camera trajectory globally consistent. In DSO [25], authors proposed a fully direct probabilistic model that integrated a full photometric calibration. By incorporating a photometric bundle adjustment, the system outperformed other state-of-the-art works in terms of both accuracy and robustness. To achieve real-time performance on a standard CPU, the authors also exploited the sparsity structures of the corresponding Hessian matrix. While the photometric model provided accurate pose estimation over short-term tracking without data association, the geometric model gave robustness for a large baseline. Hybrid approaches that used both photometric and geometric errors were proposed, with the most representative work SVO [35], proposed by Forster et al. where the short-term tracking was solved by minimizing the photometric error, while the long-term drift was constrained by a windowed bundle adjustment on visual landmarks.

There have been many discussions in the literature to answer the question: Which is better? While it is difficult to answer this question now, it is true that the direct method often shows better short-term performance in low-textured environments [25], [32]. Besides, the direct method is often more computationally efficient due to the removal of feature extraction [35]. To leverage these advantages, $R^{3} L I V E + +$ uses a photometric-based VIO subsystem. Unlike the pure visual (or visual-inertial) direct odometry systems, which perform bundle adjustment on photometric errors [25] or feature reprojection errors [35] to restrain long-term drift, the VIO in R³LIVE++ makes full use of the geometry structure reconstructed from LiDAR point cloud by minimizing the radiance errors between map points and image pixels. Such a frame-to-map alignment effectively lowers the odometry drift at a low computation cost. Moreover, pure visual (or visual-inertial) direct methods construct photometric errors on dense images [34] or a sparse set of image patches [25], [35], while the photometric errors of R³LIVE++ VIO subsystem are on a sparse set of individual pixels. Furthermore, the VIO in $R^{3} L I V E + +$ accounts for the camera photometric calibration (e.g., non-linear response function and lens vignetting) and estimates the camera exposure time online, which helps improve the odometry accuracy and recovers the true radiance information of the environment.

C. LiDAR-Visual Fused Odometry

On the basis of LiDAR-inertial methods, LiDAR-inertialvisual odometry incorporating measurements from visual sensors shows higher robustness and accuracy. Zhang and Singh in [36] proposed a LiDAR-inertial-visual system that used a loosely-coupled VIO as the motion model to initialize the Li-DAR mapping subsystem. Similarly, Shao et al. in [37] proposed a stereo visual-inertial LiDAR SLAM that incorporated the tightly-coupled stereo VIO with LiDAR mapping and LiDARenhanced visual loop closure. The overall system was still a loosely-coupled fusion since the LiDAR data were not jointly optimized along with the visual-inertial measurements.

There are also some RGB-D-inertial odometry, such as [37], [38]. Designed for RGB-D cameras, these methods are difficult to be applied on LiDARs due to the significant differences in the measurement pattern, range, and density between RGB-D cameras and LiDAR sensors. In [39], LiDAR measurements were used to provide depth information for camera images at each frame, forming a system similar to RGB-D camera and hence being able to leverage existing visual SLAM works such as ORB-SLAM2 [29]. This was also a loosely-coupled method as it ignored the direct constraints imposed by LiDAR measurements.

For the works mentioned above, the measurement of LiDAR and camera was fused in a loosely-coupled manner. To achieve higher accuracy and robustness, frameworks that fused sensor data in a tightly-coupled way were proposed in recent years. Zuo et al. [40] proposed a LIC-fusion framework combining IMU measurements, sparse visual features, and LiDAR plane and edge features with online spatial and temporal calibration based on the MSCKF framework. The system exhibited higher accuracy and robustness than other state-of-the-art methods in their experiment results. Later on, their further work termed LIC-Fusion 2.0 [41] refined a novel plane-feature tracking algorithm across multiple LiDAR scans within a sliding window to make LiDAR scan matching more robust. David et al. in [42] proposed a multi-sensor fusion framework named VILENS that tightly-coupled fuse the visual, inertial, legged, and LiDAR sensor data using a novel factor graph. The fusion of four different sensor modalities makes the system achieve better accuracy in localization and makes it robust to challenging scenarios in which individual sensors would produce degenerated estimates. Shan et al. in [43] proposed LVI-SAM that fused the LiDAR, visual, and inertial sensors in a tightly-coupled smooth and mapping framework, which was built atop a factor graph. The LiDAR-inertial and visual-inertial subsystems of LVI-SAM could function independently when failure was detected in one of them, or jointly when enough features were detected. A similar tightly-coupled system was our previous work R²LIVE [44], which fused the LiDAR and camera measurements in an onmanifold iterated Kalman filter. $R^{2} L I V E$ could run in various challenging scenarios even with small LiDAR FoV, aggressive motions, sensor failures, and narrow tunnel-like environments with moving objects.

The above LiDAR-inertial-visual systems all used featurebased methods in both LIO and VIO. In contrast, R³LIVE++ uses direct methods in both LIO and VIO to best exploit any subtle features in the environments even in case of extreme scenarios (e.g., structure-less and/or texture-less environments). Moreover, the above LiDAR-inertial-visual systems mainly focused on the localization part and has very limited consideration on the mapping efficiency and accuracy. Hence, their visual and LiDAR subsystem often maintains two separate maps for the LIO and VIO, preventing the data fusion at a deeper level and the reconstruction of high-accuracy colored 3D maps. $R^{3} L I V E + +$ is designed to perform both localization and radiance map reconstruction in real-time. The central of these two tasks is a single radiance map shared among and maintained by both LIO and VIO. In particular, the LIO subsystem reconstructs the geometric structure of the map and the VIO subsystem recovers the radiance information of the map.

TABLE I NOMENCLATURE

Notation Explanation

Expressions

/The encapsulated "boxplus" and"boxminus" operations on manifold [46]

G_{(\cdot)}

The value of (·) expressed in global frame

^{C} (\cdot)

The value of (·) expressed in camera frame

E x p (\cdot) / L o g (\cdot)

The Rodrigues' transformation between therotation matrix and rotation vector

δ (\cdot)

The estimated error of (·) parameterizedin tangent space.

Σ_{(\cdot)}

The covariance matrix of vector (·)

Variables

b_{g}, b_{a}

The bias of gyroscope and accelerometer in an IMU

G_{g}

The gravitational acceleration in global frame

G_{V}^{-}

The linear velocity in global frame

(^{G} R_{I},^{G} p_{I})

The IMU attitude and position w.r.t. global frame

(^{I} R_{C},^{I} p_{C})

The extrinsic between camera and IMUx The ground-true statex The prior estimation of xx The current estimate of x in each ESIKF iteration

δ {\overset{ˇ}{x}}_{k}^{*}

The optimal delta step of x in each iteration

This paper is an extension of the previously published work R³LIVE [45]. The extended works of this paper include 1) a full incorporation of the camera photometric calibration, which corrects the camera nonlinear response function and lens vignetting effect; 2) an online estimation of the camera exposure time. The estimated exposure time and the camera photometric calibration enables the system to recover the true radiance information ofthe environment; 3) a more comprehensive evaluation of the system on both open and self-collected dataset in terms of localization accuracy, robustness and radiance map reconstruction accuracy; and 4) release of the system codes, associated dataset, and the in-house designed hardware devices for collecting this dataset.

III. BASIC MODELS

A. Notations

In this paper, we use notations shown in Table I.

B. System Overview

To simultaneously estimate the sensor pose and reconstruct the environment radiance map, we design a tightly-coupled LiDAR-inertial-visual sensor fusion framework, as shown in

Fig. 2. The overview of our proposed system.

Fig. 2. The proposed framework contains two subsystems: the LIO subsystem (upper part) and the VIO subsystem (lower part). The LIO subsystem constructs the geometric structure of the radiance map by registering point cloud measurements of each input LiDAR scan. The VIO subsystem recovers the radiance information of the map in two steps: the frame-toframe VIO update estimates the system state by minimizing the frame-to-frame PnP reprojection error, while the frame-tomap VIO update minimizes the radiance error between map points and the current image. The two subsystems are tightly coupled within an on-manifold error-state iterated Kalman filter framework (ESIKF) [46], where the LiDAR and camera visual measurements are fused to the same system state (Section III-D) at their respective data reception time (Sections IV and V).

1) Point: Our radiance map is composed of map points in the global frame, in which each point P is a structure as below:

P = {[^{G} p_{x},^{G} p_{y},^{G} p_{z}, γ_{r}, γ_{g}, γ_{b}]}^{T} = {[^{G} p^{T}, γ^{T}]}^{T} \in R^{6}

where the head sub-vector ${ \mathbf { \xi } } ^ { G } \mathbf { p } = [ { \mathbf { \overset { G } { p } } } _ { x } , { \mathbf { \overset { G } { p } } } _ { y } , { \mathbf { \overset { G } { p } } } _ { z } ] ^ { T } \in \mathbb { R } ^ { 3 } $d e n o t e s t h e p o i n t 3 D p o s i t i o n, a n d t h e t a i l s u b - v e c t o r$ \gamma = $ $ [ \gamma _ { r } , \gamma _ { g } , \gamma _ { b } ] ^ { T } \in \mathbb { R } ^ { 3 }$ is the point radiance consisting of three independent channels (i.e., red, green, and blue channel) accounting for the camera photometric calibration (see Section III-C). Besides, we also record other necessary information of this point, such as the $3 \times 3$ matrix $Σ_{p}$ and $Σ_{γ}$ , which denote the covariance of the estimation errors of $G_{p}$ and $γ_{:}$ , respectively, and the timestamps when this point was created and updated.

2) Voxel: For fast and efficient retrieving of points in the radiance map, such as for VIO update in Section V-C and map point radiance recovery in Section V-D, we organize map points inside the fixed-size voxels (e.g., with dimensions of 0.1m $\times$ $0.1 m \times 0.1 m)$ . If a voxel has points appended recently (e.g., in recent 1 s), we mark this voxel as activated. Otherwise, this voxel is marked as deactivated.

C. Color Camera Photometric Model

A camera observes the radiance of the real world in the form of images that consists of 2D arrays of pixel intensities. In our work, we model the image formation process of a camera based on [47] and further extend the gray camera model to a color camera. As shown in Fig. 3, for a point P in the world, it reflects the incoming lights emitted from a light source (e.g., the sun). The reflected lights then pass through the camera lens and finally arrive at the CMOS sensor, which records the intensity of the reflected lights and creates a pixel channel in the output image. The recorded intensity is determined by the radiance, a measure of the power reflected at the point P.

Fig. 3. The image formation process of a color camera.

To model the above imaging process, we denote γ the radiance at point P. Since a color camera has three channels in its CMOS sensor: red, green, and blue, the radiance $γ$ has three components: $γ_{r}, γ_{g}, γ_{b}$ , respectively. For each channel i, the lights passing through the camera lens have power $O_{i} (ρ)$ as:

O_{i} (ρ) = V (ρ) γ_{i}

where $V (ρ) \in [0, 1]$ is called the vignetting factor accounting for the lens vignetting effect. Since the vignetting effect is different at different areas of the lens, the vignetting factor $V (ρ)$ is a function of the pixel location $ρ .$

$O_{i} (ρ)$ is the amount of power that can be received by the sensor and is called the irradiance. When taking an image, the captured irradiance $O (ρ)$ is integrated over time (i.e., the exposure time τ). The accumulated irradiance $θ_{i} = τ V (ρ) γ_{i}$ is then converted as the output of pixel intensity $I_{i} (ρ)$ via the camera response function (CRF) $f_{i} (\cdot)$ :

I_{i} (ρ) = f_{i} (τ V (ρ) γ_{i}), I_{i} \in [0, 255] .

Since a real camera sensor has a limited dynamic range and the physical scale of the radiance $γ$ can not be recovered anyway, the pixel intensities can be normalized within [0, 1] without loss of generality.

As noted in $(3),$ different channels often have different nonlinear response functions (CRF) $f_{i} (\cdot)$ and they can be calibrated offline along with the vignetting factor $V (ρ)$ based on the method in [47]. The exposure time $τ$ is estimated online in our work. With the calibration and estimation results, the radiance of point P from the observed pixel value $I (ρ)$ can be computed as:

γ_{i} = \frac{f_{i}^{- 1} (I_{i} (ρ))}{τ V (ρ)} .

Remark: Under the assumption of constant continuous light sources and a Lambertian reflection model, the radiance at point $P$ is a constant physical value that is invariant to the camera pose. Such invariance to time and camera pose enables us to infer the camera ego-motion from the radiance difference between the map and the current image (with photometric calibration), as detailed in Section V-C.

D. State

In our work, we define the full state x as:

x = (^{G} R_{I},^{G} p_{I},^{G} v, b_{g}, b_{a},^{G} g,^{I} R_{C},^{I} p_{C}, ϵ,^{I} t_{C}, ϕ)

where the notations ${ ^ { G } \mathbf { R } } _ { I } , { ^ { G } \mathbf { p } } _ { I } , { ^ { G } \mathbf { v } } , \mathbf { b } _ { \mathbf { g } } , \mathbf { b } _ { \mathbf { a } } , { ^ { G } \mathbf { g } } , { ^ { I } \mathbf { R } } _ { C } , { ^ { I } \mathbf { p } } _ { C } $a r e e x p l a i n e d i n T a b l e$ \mathrm { I } , \ ^ { I } t _ { C } $i s t h e t i m e - o f f s e t b e t w e e n I M U a n d c a m e r a w h i l e L i D A R i s a s s u m e d t o b e s y n c e d w i t h t h e I M U a l r e a d y,$ \epsilon = 1 / \tau $i s t h e i n v e r s e c a m e r a e x p o s u r e t i m e,$ \phi = [ f _ { x } , f _ { y } , c _ { x } , c _ { y } ] ^ { T } $a r e t h e c a m e r a i n t r i n s i c s, w h e r e$ ( f _ { x } , f _ { y } ) $d e n o t e t h e c a m e r a f o c a l l e n g t h a n d$ ( c _ { x } , c _ { y } ) $t h e o f f s e t s o f t h e p r i n c i p a l p o i n t f r o m t h e t o p - l e f t c o r n e r o f t h e i m a g e p l a n e . T h e c a m e r a e x t r i n s i c$ ( { ^ { I } { \bf R } _ { C } } , { ^ { I } { \bf \bar { p } } _ { C } } ) $, i n t r i n s i c$ \phi $a n d t i m e - o f f s e t$ ^ I t _ { C }$ would usually have their rough values available (e.g., offline calibration, CAD model, manufacturer’s manual). To cope with the possible calibration errors (e.g., extrinsic $(^{I} R_{C},^{I} p_{C})$ and intrinsic φ) or online drifting (e.g., time-offset $^{I} t_{C})$ , we also include them in the state x such that they will be estimated online. Besides, we also estimate the camera exposure time online in order to recover the true radiance value of each map point.

In R³LIVE++, our LiDAR-Inertial odometry (LIO) and visual-Inertial odometry (VIO) take advantage of the measurements data from LiDAR, IMU, and camera sensors, continuously update the full state x for achieving that goal of optimal state estimation.

IV. LIDAR-INERTIAL ODOMETRY (LIO)

Our LIO subsystem reconstructs the geometry structure of the environment by registering each new LiDAR scan to the global map. We use the generalized-iterative closest point (GICP) method [48] to iteratively estimate the LiDAR pose (and other system states) by minimizing the distance of each point in the scan to a plane fitted from the corresponding points in the map. The estimated state estimate is then used to append the new points to the map.

A. LiDAR Point-to-Plane Residual

As shown in Fig. 2, our LIO subsystem constructs the geometric structure of the global map.

For the k-th input LiDAR scan, we first compensate the inframe motion with an IMU backward propagation introduced in [23]. Let $L_{k} = {^{L} p_{1}, . . .,^{L} p_{m}}$ be the set ofm LiDAR points after motion compensation, we compute the residual of each raw point (or a downsampled subset) of $L_{p_{s}} \in L_{k}$ where s is the index of point and the superscript L denotes that the point is represented in the LiDAR-reference frame.

With ${\overset{ˇ}{x}}_{k}$ being the estimate of $x_{k}$ at the current iteration, we transform $L_{p_{s}}$ from LiDAR frame to the global frame:

{^{G} p}_{s} =^{G} {\overset{˘}{R}}_{I_{k}} (^{I} R_{L}^{L} p_{s} +^{I} p_{L}) +^{G} {\overset{˘}{p}}_{I_{k}}

To register the point to the global map, we search for the nearest five points in the map. To accelerate the nearest neighbor search, map points are organized into an incremental k-d tree (see [24]). If the euclidean distance between ${Π^{G}}_{p_{s}}$ and the searched five nearest neighbor points is smaller than a threshold (e.g., 0.4 m), these found nearest neighbor points are used to fit a plane with normal $u_{s}$ and centroid $q_{s}$ . Then, the LiDAR measurement residual $r_{l} (\overset{ˇ}{x} k,^{L} p s)$ is:

r_{l} ({\overset{ˇ}{x}}_{k}, ξ^{L} p_{s}) = u_{s}^{T} (^{G} p_{s} - q_{s}) .

B. LIO ESIKF Update

The residual in (7) should be zero ideally. However, due to the estimation error in ${\overset{ˇ}{x}}_{k}$ and the LiDAR measurement noise, this residual is often not zero and can be used to refine the state estimate ${\overset{ˇ}{x}}_{k}$ . Specifically, let $n_{s}$ be the measurement noise of the point $L_{p_{s}}$ , we have the relation between the true point location ${\dot{ξ}}_{p_{s}^{g t}}^{g t}$ and the measured one $L_{p_{s}}$ as below:

\nabla^{L} p_{s} = \nabla^{L} p_{s}^{g t} + n_{s}, n_{s} ~ N (0, Σ Σ_{n_{s}}) .

This true point location together with the true state $x_{k}$ should lead to zero residual in (7), i.e.,

\begin{matrix} 0 = r_{l} (x_{k}, ω^{L} p_{s}^{g t}) \approx r_{l} ({\overset{ˇ}{x}}_{k}, ω^{L} p_{s}) + H_{s}^{l} δ {\overset{ˇ}{x}}_{k} + α_{s} \end{matrix}

where $x_{k}$ is parameterized by its error $δ {\overset{ˇ}{x}}_{k}$ in the tangent space of $\check { \mathbf { x } } _ { k } \ ( \mathrm { i . e . , ~ } \mathbf { x } _ { k } = \check { \mathbf { x } } _ { k } \boxplus \delta \check { \mathbf { x } } _ { k } ) , \alpha _ { s } \sim \mathcal { N } ( \mathbf { 0 } , \boldsymbol \Sigma _ { \alpha _ { s } } ) $i s t h e l u m p e d n o i s e d u e t o$ \mathbf { n } _ { s } $, a n d$ \mathbf { H } _ { s } ^ { l } $i s t h e J a c o b i a n o f t h e r e s i d u a l w . r . t .$ \delta \check { \mathbf { x } } _ { k }$

Equation (9) constitutes an observation distribution for $x_{k}$ (or equivalently $δ {\overset{ˇ}{x}}_{k} ≜ x_{k} v x {\overset{ˇ}{x}}_{k})$ , which is combined with the prior distribution from the IMU propagation:

\begin{matrix} min_{δ {\overset{ˇ}{x}}_{k}} (‖ ({\overset{ˇ}{x}}_{k} ⊞ δ {\overset{ˇ}{x}}_{k}) ⊞ {\hat{x}}_{k} ‖_{Σ_{δ {\hat{x}}_{k}}}^{2} \\ + \sum_{s = 1}^{m} \| r_{l} ({\overset{ˇ}{x}}_{k},^{L} p_{s}) + H_{s}^{l} δ {\overset{ˇ}{x}}_{k} {\|}_{Σ_{α_{s}}}^{2}) \end{matrix}

where $‖ x ‖_{Σ}^{2} = x^{T} Σ^{- 1} x$ is the squared Mahalanobis distance with covariance $Σ, {\hat{x}}_{k}$ is the IMU propagated state estimate, $Σ_{δ {\hat{x}}_{k}}$ is the IMU propagated state covariance. The first item essentially represents $‖ x ⊖ {\hat{x}}_{k} ‖_{Σ_{δ {\hat{x}}_{k}}}^{2}$ , which incorporates the prior information from the IMU propagation.

Solving (10) leads to the Maximum A-Posteriori (MAP) estimate of $δ {\overset{ˇ}{x}}_{k}^{*}$ which is then added to ${\overset{ˇ}{x}}_{k}$ as below:

{\overset{ˇ}{x}}_{k} \leftarrow {\overset{ˇ}{x}}_{k} ⊞ δ {\overset{ˇ}{x}}_{k}^{*}

The above iteration process is iterated until convergence $(i . e .$ the update $δ {\overset{ˇ}{x}}_{k}^{o}$ is smaller than a given threshold). The converged state estimate ${\overset{ˇ}{x}}_{k}$ is then used as the starting point of the IMU propagation until the reception ofthe next LiDAR scan or camera image. Furthermore, the converged estimate ${\overset{ˇ}{x}}_{k}$ is used to append points in the current LiDAR scan to the global map as follows. For the s-th point $L_{p_{s}} \in L_{k}$ , its position in global frame $G_{p_{i}}$ is first obtained by (6). If ${Π^{G}}_{p_{s}}$ has nearby points in the map with distance 1 cm (see Section III-B-1), $\dot{G_{p_{s}}}$ will be discarded to maintain a spatial resolution of 1cm. Otherwise, a new point structure $P_{s}$ will be created in the map with:

P_{s} = {[^{G} p_{s}^{T}, γ_{s}^{T}]}^{T} = {[^{G} p_{s}, 0]}^{T}

where the radiance vector $γ_{s}$ is set as zero and will be initialized at the first time it is observed in forthcoming images (see Section V-D). Finally, we mark the voxel containing ${Π^{G}}_{p_{s}}$ as activated such that the radiance of points in this voxel can be updated by the forthcoming images (see Section V-D). This is because, in most LiDAR-camera platforms, such as those used in LVI-SAM [43], FAST-LIVO [49], and our system, LiDAR, and cameras are typically installed closely to achieve a larger field of view overlap. This configuration results in the points within “activated” voxels being unobscured to the current camera. Consequently, we can efficiently identify and retrieve the points that are unoccluded to the current camera since such points would also appear in current LiDAR measurements (hence labeled as “activated” for visual fusion).

V. VISUAL-INERTIAL ODOMETRY (VIO)

While our LIO subsystem reconstructs the geometric structure of the environment, our VIO subsystem recovers the radiance information from the input color images. To be more specific, our VIO subsystem projects a certain number of points (i.e., tracked points) from the global map to the current image, then it iteratively estimates the camera pose (and other system states) by minimizing the radiance error of these points. Only a sparse set of tracked map points is used for the sake of computation efficiency.

Our proposed framework is different from previous photometric-based methods [35], [50], which constitute the residual of a point by considering the photometric error over all its neighborhood pixels (i.e., a patch). These patch-based methods achieve stronger robustness and faster convergence speed than those without. However, the patch-based method is not invariant to either translation or rotation, which requires estimating the relative transform when aligning one patch to another. Plus, the calculation of the residual is not completely precise by assuming the depths of all pixels in the patch are the same as the mid-point. On the other hand, our VIO is operated at an individual pixel, which utilizes the radiance of a single map point to compute the residual. The radiance, which is updated simultaneously in the VIO, is an inherent property of a point in the world and is invariant to both camera translation and rotation. To ensure a robust and fast convergence, we design a two-step pipeline shown in Fig. 2. Specifically, in the first step (i.e., frame-to-frame VIO update), we leverage a frame-to-frame optical flow to track map points observed in the last frame and obtain a rough estimate of the system’s state by minimizing the Perspective-n-Point (PnP) reprojection error of the tracked points (Section V-B). Then, in the second step (i.e., frame-to-map VIO), the state estimate is further refined by minimizing the difference between the radiance of map points and the pixel intensities at their projected location in the current image (Section V-C). With the converged state estimate and the raw input image, we finally update the map points radiance according to the current image measurement (Section V-D).

A. Photometric Correction

For each incoming image I, we first correct the image nonlinear CRF $f_{i} (\cdot)$ and the vignetting factor $V (\cdot)$ , which are calibrated in advance (see Section III-C), to obtain the photometrically corrected image Γ, whose i-th channel at pixel location $ρ$ is:

Γ_{i} (ρ) = \frac{f_{i}^{- 1} (I_{i} (ρ))}{V (ρ)} .

The photometrically corrected image Γ is then used in the following VIO pipelines including the frame-to-frame VIO, frame-to-map VIO and radiance recovery.

B. Frame-to-Frame Visual-Inertial Odometry

As depicted in Fig. 4, we leverage Lucas − Kanade optical flow for tracking a certain number (e.g., 400) of map points (i.e., tracked points). These tracked points are initially created by projecting map points onto the first camera images while maintaining a minimum distance (e.g., 50 pixels) between adjacent points. These tracked map points are then employed in both the frame-to-frame and frame-to-map VIO update processes. Following the VIO update, the tracked points are updated accordingly, as will be introduced in Section V-E.

1) Perspective-N-Point Reprojection Error: Assume we have tracked m map points $P = {P_{1}, . . ., P_{m}}$ in the last image frame $I_{k - 1}$ with their projected location in $I_{k - 1}$ being ${ρ_{1_{k - 1}}, . . ., ρ_{m_{k - 1}}}$ , we leverage the Lucas − Kanade optical flow to find out their corresponding location in the current image $I_{k} .$ , denoted as ${ρ_{1_{k}}, . . ., ρ_{m_{k}}}$ . Then, we iteratively minimize the reprojection errors of to obtain a rough estimate of the state (see Fig. 5). Specifically, taking the s-th point $P_{s} =$ $[^{G} p_{s}^{T}, γ_{s}^{T}]^{T} \in P$ as an example, let ${\overset{ˇ}{x}}_{k}$ be the state estimate at the current iteration, the projection error $r_{c} ({\overset{ˇ}{x}}_{k}, ρ_{s_{k}}, σ_{p_{s}})$ is

r_{c} ({\overset{ˇ}{x}}_{k}, ρ_{s_{k}}, Π^{G} p_{s}) = ρ_{s_{k}} - π ({\overset{ˇ}{Δ}}^{G} p_{s}, {\overset{ˇ}{x}}_{k})

Fig. 4. The colored “tadpole”-shaped dots represent the tracked points $P$ that are used for VIO updates, which are tracked using the Lucas − Kanade optical flow. The head position of each map point corresponds to its location in the current image frame $I_{k},$ , while the tail position corresponds to its location in the previous image frame $I_{k - 1}$ . Among these tracked points, the solid white dots represent historical tracked points that have been tracked in several previous frames, while the solid black dots represent the new points that were added in the last image frame $I_{k - 1}$

Fig. 5. Frame-to-frame VIO estimates the system’s state by minimizing the PnP reprojection error of map points observed in the last frame.

where $π (^{G} p_{s}, {\overset{˘}{x}}_{k}) \in R^{2}$ is the predicted pixel location computed as below:

π ({^{G} p}_{s}, {\overset{ˇ}{x}}_{k}) = π_{p h} ({^{G} p}_{s}, {\overset{ˇ}{x}}_{k}) + {^{I} \overset{ˇ}{t}}_{C_{k}} \cdot \frac{(ρ_{s_{k}} - ρ_{s_{k - 1}})}{Δ t_{k - 1, k}}

where the first term $π_{p h} (G_{p_{s}, {\overset{ˇ}{x}}_{k}})$ is the standard camera pinhole model, the second one is the temporal correction factor [51], and $Δ t_{k - 1, k}$ is the time interval between the last and current image. Since $Δ t_{k - 1, k}$ is small $(e . g ., < 50 m s)$ , we estimated the value of $^{I} {\overset{ˇ}{t}}_{C_{k}}$ by assuming that the projected position of $G_{p_{s}}$ moves at a constant velocity of $(ρ_{s_{k}} - ρ_{s_{k - 1}}) / Δ t_{k - 1, k}$ on image plane during the interval between two consecutive frames.

2) Frame-to-Frame VIO Update: Similar to the LIO update, the state estimation error in ${\overset{ˇ}{x}}_{k}$ and the camera measurement noise will lead to a certain residual in (14), from which we can update the state estimate ${\overset{ˇ}{x}}_{k}$ as follows. First, the measurement noise in the residual (14) consists of two sources: one is the pixel tracking error in $ρ_{s_{k}}$ and the other lies in the map point location error $Π^{G} p_{s}$ ,

Λ^{G} p_{s} = Λ^{G} p_{s}^{g t} + {n_{p}}_{s}, n_{p_{s}} ~ N (0, Σ_{{n_{p}}_{s}})

ρ_{s_{k}} = ρ_{s_{k}}^{g t} + n_{ρ_{s_{k}}}, n_{ρ_{s_{k}}} ~ N (0, Σ_{n_{ρ_{s_{k}}}})

Fig. 6. Frame-to-map VIO refines the state estimate by minimizing the radiance error between the map point and the observed radiance in the current image.

where $G_{p_{s}^{g t}}$ and $ρ_{s_{k}}^{g t}$ are the true values of $G_{p_{s}}$ and $ρ_{s_{k}}$ respectively. Then, correcting such noises and using the true system state should lead to zero residual, i.e.,

0 = r_{c} (x_{k}, ρ_{s_{k}}^{g t}, Λ_{}^{G} p_{s}^{g t}) \approx r_{c} ({\overset{˘}{x}}_{k}, ρ_{s_{k}}, Λ_{}^{G} p_{s}) + H_{s}^{r} δ {\overset{˘}{x}}_{k} + β 18)

where $H_{s}^{r}$ is the Jacobian of the residual w.r.t. $δ {\overset{ˇ}{x}}_{k}$ and $β_{s} ~$ $N (0, Σ_{β_{s}})$ is the lumped noise due to $n_{p_{s}}$ and $n_{ρ_{s}}$

Equation (18) constitutes an observation distribution for $x_{k} .$ which is combined with the IMU propagation to obtain the MAP estimate of the state in the same way as the LIO update detailed in Section IV-B. The converged state estimate is then refined in the frame-to-map VIO in the next section.

Remark: Since the camera pin-hole model $π_{p h} (G_{p_{s}, x_{k}})$ in (15) is related to camera pose (consisting of the IMU pose $(^{G} R_{I},^{G} p_{I})$ and camera extrinsic $(^{I} R_{C},^{\bar{I}} p_{C}))$ and intrinsic $ϕ,$ , so the projection model $π (G_{p_{s}, x_{k}})$ is also related to these state components. In addition, $π (G_{p_{s}, x_{k}})$ is also related to the temporal offset $^{I} t_{C}$ due to the temporal correction factor. This will cause H^r to contain nonzero elements corresponding to the IMU pose $({\bar{Γ}}^{\bar{G}} R_{I}, {\bar{Γ}}^{G} p_{I})$ , camera extrinsic $(^{I} R_{C},^{I} p_{C})$ , intrinsic φ, and temporal offset $^{I} t_{C}$ , and hence an update of them in the state estimation.

C. Frame-to-Map Visual-Inertial Odometry

1) Frame-to-Map Radiance Error: The frame-to-frame VIO update can provide a good state estimate ${\overset{ˇ}{x}}_{k}$ , which is further refined by the frame-to-map VIO update by minimizing the radiance error ofthe tracked map points . Let $T_{k}$ the photometrically calibrated image at the k-th step (see (13)). With the state estimate at the current iteration, ${\overset{ˇ}{x}}_{k} .$ , which contains the estimated camera pose, extrinsic, intrinsic, and exposure time, we project a tracked map point $P_{s} \in P$ to the image plane to obtain its pixel location ${\overset{˘}{ρ}}_{s_{k}} = π (^{G} p_{s}, {\overset{˘}{x}}_{k})$ (see (15) and Fig. 6). Then, the observed radiance denoted by $Φ_{s}$ can be computed from the exposure time component ${\overset{ˇ}{ϵ}}_{k}$ in xˇ_k as: $Φ_{s} = {\overset{ˇ}{ϵ}}_{k} Γ_{k} ({\overset{ˇ}{ρ}}_{s_{k}})$ . Finally, the frame-to-map radiance error is the difference between the radiance component $γ_{s}$ of the point $P_{s}$ and the observed value

Φ_s:

\begin{matrix} r_{c} ({\overset{ˇ}{x}}_{k},^{G} p_{s}, γ_{s}) = Φ_{s} - γ_{s}, Φ_{s} = {\overset{ˇ}{ϵ}}_{k} r_{k} ({\overset{ˇ}{ρ}}_{s_{k}}), \end{matrix}

where $Φ_{s}, T_{k} ({\overset{ˇ}{ρ}}_{s_{k}})$ and $γ_{s}$ both contain three channels: red, green, and blue.

2) Frame-to-Map VIO Update: The measurement noise in (19) come from both the component $γ_{s}$ and $Φ_{s} .$ . For the component $γ_{s} .$ , we model it as:

\begin{matrix} γ_{s} = γ_{s}^{g t} + n_{γ_{s}} + n_{i c}, n_{γ_{s}} ~ N (0, Σ_{n_{γ_{s}}}) \\ n_{i c} ~ N (0, σ_{i c}^{2} \cdot Δ t_{γ_{s}}) \end{matrix}

where $γ_{s}^{g t}$ represents the ground-truth value of $γ s$ . The terms $n_{γ_{s}}$ originate from the radiance estimation error as detailed in Section $V - D,$ and $n_{i c}$ arises from radiance temporal changes due to variations in illumination. Given that illumination changes usually occur gradually over time, we model this process as a random walk [52], whose covariance is linear to the time interval $Δ t_{γ_{s}}$ between current time and last update time of $P_{s}$ . Since $n_{γ_{s}}$ and $n_{i c}$ are indeed two independent noise, we have the Gaussian distribution of $γ_{s}$ as $\gamma _ { s } \sim \mathcal { N } ( \gamma _ { s } ^ { g t } , \pmb { \Sigma _ { n _ { \gamma _ { s } } } } + \pmb { \sigma } _ { \mathrm { i c } } ^ { 2 } \cdot \Delta t _ { \gamma _ { s } } )$

For the second component $Φ_{s}$ in (19), it is computed from the state estimate ${\overset{ˇ}{x}}_{k}$ and the current image $T_{k}$ as $Φ_{s} =$ ${\overset{ˇ}{ϵ}}_{k} Γ_{k} (π (^{G} p_{s}, {\overset{ˇ}{x}}_{k}))$ , hence its noise consists of two sources: one is the state estimation error (from ${\overset{ˇ}{x}}_{k})$ and the other is the image measurement noise (from $T_{k})$ :

Φ_{s} = Φ_{s}^{g t} + n_{Φ_{s}}, n_{Φ_{s}} ~ N (0, Σ_{n_{Φ_{s}}})

where $Σ_{n_{Φ, s}}$ denotes the covariance due to these two noise sources.

Combining (19), (20) and (21), we obtain the first order Taylor expansion of the true zero residual $r_{c} (x_{k},^{G} p_{s}^{g t}, γ_{s}^{g t})$ :

0 = r_{c} (x_{k},^{G} p_{s}^{g t}, γ_{s}^{g t}) \approx r_{c} ({\overset{˘}{x}}_{k},^{G} p_{s}, c_{s}) + H_{s}^{c} δ {\overset{˘}{x}}_{k} + ζ_{s}

where $H_{s}^{r}$ is the Jacobian of the residual w.r.t. $δ {\overset{ˇ}{x}}_{k}$ and $ζ_{s} ~$ $N (0, Σ_{ζ_{s}})$ is the lumped noise due to noises in $γ_{s}$ and $Φ_{s}$

Similar as before, (22) constitutes an observation distribution for state $x_{k}$ , which is combined with the IMU propagation to obtain the MAP estimate of the state.

Remark: Since the $Φ_{s}$ in (19) is related to camera exposure time $ϵ,$ it will cause $H_{s}^{c}$ to contain nonzero elements corresponding to the exposure time and hence an update of them in the state estimation.

D. Recovery of Radiance Information

After the frame-to-map VIO update, we have the precise pose of the current image. Then, we perform the Bayesian update to determine the optimal radiance of all map points such that the average radiance error between each point and its viewed images is minimal.

First of all, we retrieve all the points in all activated voxels (activated in Section IV-B). Assume the retrieved point set is $Q = {P_{1}, . . ., P_{n}}$ . For the s-th point $P_{s} = [^{G} p_{s}^{T}, γ_{s}^{T}]^{T} \in Q$ falling in the current image FoV, we first can obtain the observed radiance vector $Φ_{s}$ by (19) and its covariance $Σ_{n_{Φ_{s}}}$ by (21). To prevent the underestimation of the radiance vector caused by underexposure or overexposure, we exclude points in $Q$ from the update process if their pixel values in any RGB channel are the minimum or maximum value (i.e., 0 or 255).

Fig. 7. We update the radiance $γ_{s}$ of a map point via Bayesian update.

If $P_{s}$ is a new point appended by the LIO subsystem (see Section IV-B) with $γ_{s} = 0 .$ , we set:

γ_{s} = Φ_{s}, Σ_{n_{γ_{s}}} = Σ_{n_{Φ_{s}}}

Otherwise, the radiance vector $γ_{s}$ saved in the map (see (20)) is fused with newly observed radiance vector $Φ_{s}$ with covariance $Σ_{n_{Φ, s}}$ via Bayesian update (see Fig. 7):

\begin{matrix} Σ_{n_{{\tilde{γ}}_{s}}} = {({(Σ_{n_{{\tilde{γ}}_{s}}} + σ_{i c}^{2} \cdot Δ t_{γ_{s}})}^{- 1} + Σ_{n_{Φ_{s}}}^{- 1})}^{- 1} \end{matrix}

\begin{matrix} {\tilde{γ}}_{s} = {({(Σ_{n_{γ_{s}}} + σ_{i c}^{2} \cdot Δ t_{γ_{s}})}^{- 1} γ_{s} + Σ_{n_{Φ_{s}}}^{- 1} Φ_{s})}^{- 1} Σ_{n_{{\tilde{γ}}_{s}}} \end{matrix}

γ_{s} = {\tilde{γ}}_{s}, Σ_{n_{{\tilde{γ}}_{s}}} = Σ_{n_{{\tilde{γ}}_{s}}}

E. Update of the Tracking Points

After the recovery of radiance information, we update the tracked point set $P$ for the next frame of image use. First, we remove points from current if their projection error in (14) or radiance error in (19) are too large, and also remove the points which do not fall into the current image FoV. Second, we project each point in to the current image and add it to $P$ if no other tracked points already existed in a neighborhood of 50 pixels.

VI. EXPERIMENTS

In this chapter, we conduct extensive experiments to validate the advantages of our proposed system against other counterparts in threefold: 1) To verify the accuracy in localization, we quantitatively compare our system against existing state-ofthe-art SLAM systems on a public dataset (NCLT-dataset). 2) To validate the robustness of our framework, we test it under various challenging scenarios where camera and LiDAR sensor degeneration occurs. 3) To evaluate the accuracy of our system in reconstructing the radiance map, we compare it against existing baselines in estimating the camera exposure time and calculating the average photometric error w.r.t. each image. In the experiments, two datasets are used for evaluation: the NCLT-dataset and the R³LIVE-dataset.

A. NCLT-Dataset

To compare the accuracy of our proposed method against other state-of-the-art SLAM systems, we perform quantitative evaluations on NCLT-dataset [53]. NCLT-dataset is a largescale, long-term autonomy dataset for robotics research that was collected on the University of Michigan’s North Campus. The dataset is comprised of 27 sequences that are collected by exploring the campus, both indoors and outdoors, on varying paths, and at different times of the day across all four seasons. Each sequence includes data from the omnidirectional camera, 3D LiDAR, planar LiDAR, GPS, and wheel encoders on a Segway robot.

We chose NCLT-dataset for three reasons: 1) NCLT-dataset is currently the largest public dataset with ground-truth trajectories of high quality. 2) NCLT-dataset provides all raw data sampled by the sensors, which meets our requirement for the input data. 3) NCLT-dataset has many challenging scenarios, such as moving obstacles (e.g., pedestrians, bicyclists, and cars), illumination changes, varying viewpoints, seasonal and weather changes (e.g., falling leaves and snow), and long-term structural changes caused by construction projects. For details of challenging input data from both LiDAR and camera sensors, please refer to Section 1 of our Supplementary Material [54].

In the experiments, the front-facing camera data (one of five) and the 3D LiDAR data are used for all systems under evaluation. Moreover, we notice some time synchronization errors in two sequences (i.e., 2012-03-17 and 2012-08-04), where the LiDAR timestamp is 100 ms delayed from the IMU timestamp (about one LiDAR-frame). Therefore, we exclude these two sequences from the evaluation. As a result, 25 sequences are evaluated with total traveling length up to 138 km and duration up to 33h:34m.

B. Self-Collected Dataset: R³LIVE-Dataset

While the large-scale NCLT-dataset is suitable for evaluating the localization accuracy, it didn’t cover any scenarios with sensor degeneration, preventing us from evaluating the system robustness, which is one of the major motivations of this work. Moreover, the camera photometric calibration and the groundtrue exposure time are not available in the NCLT-dataset, which are essential for the reconstruction of the radiance maps and the evaluation of the online exposure time estimation. To fill this gap, we designed a handheld data collection device and made a new dataset named R³LIVE-dataset. The dataset and hardware device are released along with the codes of this work to facilitate the reproduction of our work.

1) Handheld Devicefor Data Collection: Our handheld device for data collection is shown in Fig. 8(a), which includes a power supply unit, an onboard computer DJI manifold-2c (equipped with an Intel i7-8550u CPU and 8GB RAM), a FLIR Blackfly BFS-u3-13y3c global shutter camera, and a LiVOXAVIA 3D LiDAR. The camera FoV is $82 . 9^{\circ} \times 66 . 5^{\circ}$ and the LiDAR FoV is $70 . 4^{\circ} \times 77 . 2^{\circ}$ . To quantitatively evaluate the accuracy of our algorithm (Section VI-E) even in GPS-denied environments, we use an ArUco marker [55] as a reference to calculate the sensor pose when returns to the starting point, which enables to evaluate the localization drift. All the mechanical components of this device are designed for compatibility with fused deposition modeling (FDM) 3D printing technology. Their design schematics are open-sourced along with the codes too [56].

Fig. 8. (a) shows our handheld device for data collection. (b) shows the ArUco marker board to provide the ground-truth for evaluating the system accuracy. (c) shows our open-source schematics model.

Fig. 9. The left figure shows the calibrated nonlinear response function in three channels (red, green, and blue). The right one plots the calibrated vignetting factors at each image pixel.

To correct the camera’s nonlinear response function and the vignette effect, we perform photometric calibration on the camera based on the method in [47]. The calibrated results are shown in Fig. 9, which are also released on our GitHub repository [56].

2) The R³LIVE-Dataset: The R³LIVE-dataset was collected within the campuses of the University of Hong Kong (HKU) and the Hong Kong University of Science and Technology (HKUST). As summarized in Table II, the dataset includes 13 sequences that are collected by exploring both indoor and outdoor environments, in various scenes (e.g., walkway, park, forest, etc) at different time in a day (i.e., morning, noon, and evening). This allows the dataset to capture both structured urban buildings and cluttered field environments with different lighting conditions. The dataset also includes three sequences (degenerate_seq_00/01/02) where the LiDAR or camera (or both) degenerate by occasionally facing the device to a single and/or texture-less plane (e.g., wall, the ground) or visually. The total traveling length reaches 8.4 km, duration reaching 2.4h. More details of each sequence will be provided in sequel when it is used.

C. System Configurations

For the sake of fair comparison, in the evaluation of our systems and their counterparts, each system uses the same parameters for all sequences in the same dataset. For the counterpart systems (e.g., LIO-SAM [20], LVI-SAM [43], FAST-LIVO [49], FAST-LIO2 [24], etc.), we use their default configurations on their GitHub repository except for some necessary adjustments to match the hardware setup. For our system, we also make its configuration available on our GitHub repository [56], “https://github.com/hkumars/r3live/blob/master/config/r3live_config.yaml” for NCLTdataset and “https://github.com/hku-mars/r3live/blob/master/ config/r3live_config.yaml” for R³LIVE-dataset.

TABLE II
OVERVIEW OF THE R³LIVE-DATASET

Sequence	Duration (s)	Traveling Length (m)	Sensor Degeneration	Return to origin1	ArUco marker²	Camera exposure time³	Scenarios
degenerate_seq_00	101	74.9	Camera, LiDAR	√			Indoor
degenerate_seq_01	86	53.3	LiDAR	V			Outdoor
degenerate_seq_02	85	75.2	LiDAR	√			Outdoor
hku_campus_seq_00	202	190.6		√			Indoor
hku_campus_seq_01	304	374.6					Outdoor
hku_campus_seq_02	323	354.3		V		√	Indoor, Outdoor
hku_campus_seq_03	173	181.2		L		√	Indoor, Outdoor
hku_main_building	1170	1036.9		V		V	Indoor, Outdoor
hku_park_00	351	401.8		V	√		Outdoor, Cluttered
hku_park_01	228	247.3			√		Outdoor, Cluttered
hkust_campus_00	1073	1317.2		V	√		Indoor, Outdoor
hkust_campus_01	1162	1524.3			√		Indoor, Outdoor
hkust_campus_02	478	503.8					Indoor, Outdoor
hkust_campus_03	1618	2112.2				√	Outdoor
Total	7354	8447.6

1 Sequences are collected by traveling a loop, with starting from and ending with the same position
2 Sequences with ArUco marker [55] for providing the ground-truth relative pose.
3 Sequences with ground-truth camera exposure time read from camera's API.

D. Experiment-1: Evaluation of Localization Accuracy

In this experiment, we benchmark the localization accuracy of our systems against other state-of-the-art odometry, including LIO-SAM [20], LVI-SAM [43], FAST-LIVO [49], FAST-LIO2 [24], and our previous work R²LIVE [44], on the NCLTdataset [53]. LIO-SAM and FAST-LIO2 are LiDAR-inertial systems without fusing image data (Section II-A), while LVI-SAM, FAST-LIVO, and R²LIVE are three other state-of-the-art LiDARinertial-visual systems. Since our work is a state estimator without any loop detection and correction, we deactivated the loop closure of LIO-SAM and LVI-SAM for the sake of fair comparison. Due to the unavailability of camera photometric calibration for the NCLT dataset, we disable the photometric calibration modules of our VIO-subsystem by using V (·) = 1 and f_i(·) = 1.

Table III shows the absolute position error (APE) [57] of these methods, where Our-LIO is the LIO subsystem of our system. LIO-SAM and LVI-SAM failed in some sequences, and these sequences are excluded from the computation of the average APE. Since our method (see Fig. 2) and R²LIVE [44] output three types of odometry, one at LiDAR input, one at camera input, and one at the IMU input, while the other tested methods (e.g., LIO-SAM, FAST-LIO) outputs odometry only at LiDAR input. To ensure fairness, we compute the APE all using the odometry output at LiDAR input, even for our method and R²LIVE [44]. - As can be seen from this table, with the average APE only 8.51m, our proposed system achieves the best overall performance than other LiDAR-inertial-visual systems FAST-LIVO, R²LIVE, and LVI-SAM. The performance improvement mainly comes from the direct method used in the LIO subsystem and the tight-coupling of the LIO and VIO subsystems, the former can be seen by comparing the direct method FAST-LIO2 to the feature-based method LIO-SAM in Table III (and also detailed in [24]), the latter improves the accuracy of the VIO subsystem (hence the complete system) by leveraging the high-accuracy geometry structure reconstructed from the LiDAR. Furthermore, the overall APE of our system is lower than its LIO subsystem Our-LIO and the other LIO systems (i.e., FAST-LIO2 and LIO-SAM), which confirms the effectiveness of fusing camera data. Indeed, we found that in some evaluated sequences, the LiDAR sensor may occasionally face the sun. This can lead to the generation of noisy LiDAR points due to Sunlight contamination [58] as shown in Fig. 1 of our Supplementary Material [54], which adversely affects the accuracy of the LIO. This is due to the input images that are under-exposure (a ∼ d), over-exposure, as well as the presence of moving objects, as detailed in Fig. 2 of our Supplementary Material [54], which may adversely affect the VIO (hence the overall system). In Fig. 11, we overlay all the 25 ground-true trajectories (in the left figure) and ours (the right one). As can be seen, the overlaid trajectories estimated by our system agree with the ground-truth well and each trajectory can still be clearly distinguished without noticeable errors. Note that these 25 trajectories are collected on the same campus area across different times in a day and seasons in a year, still our system can produce consistent and reliable trajectory estimation with these illumination and scene changes, demonstrating the robustness of our system.

Given the fact that the photometric calibration of NCLTdataset is unavailable, to assess the impact of photometric calibration on localization accuracy of our method, we compared its performance with and without photometric calibration using the R³LIVE-dataset, referred to as “Our” and “Our w/o PC” in Table IV, respectively. We calculated the end-point positioning errors by comparing our results with the ground-truth estimated from an ArUco marker [55]. As shown in Table IV, photometric calibration has a minor impact on our localization outcomes on 5 sequence we tested, where the observations from our LIO and frame-to-frame VIO update primarily contribute to localization accuracy. Yet, photometric calibration is necessary for recovering the accurate radiance map of the environment, as will be illustrated in Section VI-F.

TABLE III
THE COMPARISON OF ABSOLUTE POSITION ERRORS (APE, IN METER) AND THEIR STANDARD DEVIATIONS (STD) AMONG DIFFERENCE METHODS TESTED ON NCLT-DATASET (APE ± STD)

Sequence (date)	Length (m)	Duration (hr:min:sec)	Our	$R^{2} L I V E$		LVI-SAM FAST-LIVO	Our_LIO		Fast-LIO2 LIO-SAM
2012-01-08	6495.7	01:25:35	$10.8 \pm 2.7$	$22.4 \pm 3.5$	$23.4 \pm 3.7$	$13.4 \pm 2.9$	$20.1 \pm 3.3$		$18.5 \pm 3.3$	$21.7 \pm 3.6$
2012-01-15	7499.8	01:52:19	$6.6 \pm 1.8$	$5.1 \pm 1.5$	$-$	$8.5 \pm 2.5$	$6.2 \pm 2.0$		$4.8 \pm 1.5$
2012-01-22	6183.1	01:27:22	$9.2 \pm 2.5$	$12.6 \pm 2.7$	$8.3 \pm 2.8$	$14.1 \pm 3.0$	$12.4 \pm 2.6$		$7.1 \pm 1.9$	$9.0 \pm 2.6$
2012-02-02	6315.8	01:38:36	$5.3 \pm 1.7$	$6.1 \pm 2.0$	$18.1 \pm 2.4$	$12.0 \pm 2.5$	$7.4 \pm 2.2$		$9.1 \pm 2.3$	$15.6 \pm 2.6$
2012-02-04	5641.0	01:18:30	$5.6 \pm 1.8$	$8.4 \pm 2.2$	$9.6 \pm 5.0$	$6.2 \pm 1.8$	$7.8 \pm 2.1$	$7.2 \pm 1.8$		$10.8 \pm 5.3$
2012-02-05	6649.3	01:34:17	$8.5 \pm 2.1$	$7.6 \pm 2.0$		$8.2 \pm 2.2$	$7.7 \pm 2.5$		$7.8 \pm 2.4$
2012-02-12	5829.1	01:25:35	$4.5 \pm 1.6$	$6.5 \pm 2.2$	$40.0 \pm 2.9$	$16.4 \pm 3.4$	$10.5 \pm 2.8$		$8.3 \pm 2.6$	$45.0 \pm 2.8$
2012-02-18	6249.2	01:29:55	$40.5 \pm 3.9$	$59.3 \pm 4.8$		$43.0 \pm 4.1$	$53.5 \pm 4.5$		$57.0 \pm 4.6$
2012-02-19	6232.7	01:29:11	$8.5 \pm 2.0$	$6.6 \pm 1.6$	$8.9 \pm 1.9$	$8.6 \pm 2.1$	$6.2 \pm 1.4$		$6.0 \pm 1.3$	$9.6 \pm 1.8$
2012-03-17	5907.2	01:22:53	$4.8 \pm 1.8$	$5.9 \pm 2.0$	$12.6 \pm 1.8$	$5.3 \pm 1.9$	$6.8 \pm 2.2$		$4.7 \pm 1.8$	$11.8 \pm 1.8$
2012-03-31	6073.7	01:27:53	$4.9 \pm 1.9$	$10.0 \pm 2.5$	$19.0 \pm 3.2$	$5.2 \pm 2.0$	$9.3 \pm 2.4$		$7.3 \pm 2.3$	$18.3 \pm 3.2$
2012-04-29	3183.1	00:43:18	$6.3 \pm 2.2$	$6.4 \pm 2.0$	$5.9 \pm 2.6$	$7.7 \pm 2.2$	$6.3 \pm 2.2$		$6.4 \pm 2.0$	$5.7 \pm 2.2$
2012-05-11	6116.7	01:25:05	$3.7 \pm 1.6$	$3.8 \pm 1.6$	$4.2 \pm 2.9$	$4.9 \pm 1.9$	$4.2 \pm 1.7$		$4.1 \pm 1.7$	$4.2 \pm 2.9$
2012-05-26	6340.7	01:28:34	$4.6 \pm 1.6$	$6.3 \pm 1.9$	$18.3 \pm 4.4$	$7.4 \pm 2.1$	$6.1 \pm 1.9$		$6.4 \pm 1.9$	$18.4 \pm 4.5$
2012-06-15	4085.9	00:55:10	$7.7 \pm 2.1$	$6.3 \pm 1.8$		$7.9 \pm 2.1$	$5.7 \pm 1.4$		$5.3 \pm 1.4$
2012-08-04	5492.1	01:20:32	$3.9 \pm 1.4$	$3.7 \pm 1.4$	$11.0 \pm 3.6$	$5.0 \pm 1.5$	$4.5 \pm 1.5$		$7.0 \pm 2.1$	$12.7 \pm 3.8$
2012-08-20	6014.5	01:23:48	$4.5 \pm 1.8$	$4.5 \pm 1.8$	$11.2 \pm 1.8$	$5.0 \pm 1.9$	$4.2 \pm 1.7$		$6.0 \pm 1.8$	$11.4 \pm 1.8$
2012-09-28	5574.4	01:17:59	$7.9 \pm 1.8$	$6.6 \pm 1.7$	$34.4 \pm 1.7$	$8.6 \pm 1.6$	$6.8 \pm 1.9$		$10.4 \pm 2.4$	$36.7 \pm 1.8$
2012-10-28	5682.1	01:26:10	$7.7 \pm 2.0$	$8.0 \pm 2.0$		$8.0 \pm 2.0$	$8.6 \pm 2.0$		$7.7 \pm 2.0$
2012-11-04	4788.3	01:20:39	$7.5 \pm 2.7$	$9.3 \pm 3.0$	$3.4 \pm 3.0$	$8.1 \pm 2.8$	$12.6 \pm 3.5$		$3.3 \pm 1.5$	$3.4 \pm 2.8$
2012-11-17	5751.9	01:29:44	$8.7 \pm 2.1$	$6.5 \pm 1.7$	$21.9 \pm 1.7$	$8.2 \pm 2.4$	$6.1 \pm 1.6$		$5.8 \pm 1.6$	$24.2 \pm 2.4$
2012-12-01	4991.9	01:16:48	$11.3 \pm 2.2$	$14.2 \pm 2.4$	$6.9 \pm 3.1$	$11.8 \pm 2.2$	$17.0 \pm 2.7$		$7.4 \pm 1.9$	$7.2 \pm 2.7$
2013-01-10	1137.3	00:17:04	$3.4 \pm 1.6$	$4.6 \pm 1.9$	$4.9 \pm 1.8$	$3.7 \pm 1.7$	$5.3 \pm 2.0$		$3.5 \pm 1.6$	$5.1 \pm 1.9$
2013-02-23	5235.3	01:20:08	$11.6 \pm 1.9$	$13.4 \pm 2.1$	$12.6 \pm 2.0$	$12.2 \pm 2.0$	$13.8 \pm 2.1$		$11.9 \pm 2.1$	$12.2 \pm 3.4$
2013-04-05	4523.7	01:09:27	$8.8 \pm 2.2$	$11.9 \pm 2.6$	$9.8 \pm 2.5$	$10.4 \pm 2.4$	$11.6 \pm 2.5$		$6.4 \pm 2.2$	$9.0 \pm 2.5$
Total:	137994.5	33:34:52
Average			$8.5 \pm 2.1$	$10.6 \pm 2.3$	$15.0 \pm 2.9$	$10.3 \pm 2.4$	$10.8 \pm 2.4$		$9.6 \pm 2.2$	$15.4 \pm 3.0$

1 Some systems fail in midway in some sequences and are marked as $^{''} -^{''} .$
TABLE IV

THE COMPARISONS OF END-POINT POSITIONING ERROR OF $R^{3} L I V E +$ WITH (“OUR”) AND WITHOUT PHOTOMETRIC CALIBRATION (“OUR W/O PC”) ON R³LIVE-DATASET

Sequence					degenerate_seq_02 hku_park_00 hku_park_01 hkust_campus_00 hkust_campus_01
Traveling Length (m)	$\overset{―}{75.2}$	$\overset{―}{401.8}$	$\overset{―}{247.3}$	$\overset{―}{1317.2}$	$\overset{―}{1524.3}$
Our (deg / m)	0.037 / 2.12	$0.120 / 3.82$	$0.122 / 1.97$	$2.12 : / : 6.02$	$1.53 / 5.32$
Our w/o PC (deg / m)	0.036 / 2.12	$0.118 / 3.83$	$0.128 / 2.05$	$2.23 : / : 6.22$	$1.47 / 5.26$

E. Experiment-2: Evaluation of Robustness

Besides illumination and scene change, we also test the robustness of our system to extreme scenarios where sensor degeneration occurs. We use the R³LIVE-dataset, which contains such extreme scenarios.

1) Evaluation ofRobustness in LiDAR Degenerated Scenarios: In this experiment, we evaluate the robustness of our proposed system by testing our system on the sequence “degenerate_seq_00” and “degenerate_seq $_{-} 0 1^{\dag}$ of R³LIVE-dataset (see Section VI-B2). These two sequences were collected in front ofa stairway with the LiDAR occasionally facing against the ground and a side wall (see Fig. 10(a) and (b)). When facing a wall, the LiDAR only observes a single plane, which is insufficient to determine the LiDAR pose, causing LiDAR degeneration. The device starts from and ends at the same location, enabling the

Fig. 10. Robustness evaluation in LiDAR degenerated environments.

Fig. 11. The overlay of ground-truth trajectory and ours on NCLT-dataset. The accompanying video of R³LIVE++ in reconstructing the large-scale radiance map of a sequence from the NCLT dataset is available on YouTube: https://youtu.be/kZ8_7k3HpIk?t=309.

evaluation of localization drift. The estimated trajectories of our proposed system, our LIO-subsystem Our-LIO, and another two LIO systems, FAST-LIO2 and LIO-SAM, are shown in Fig. 10(c) and (d). In addition, to further investigate the effectiveness of our frame-to-frame VIO update (in Section V-B), we conducted experiments by testing our system without the frame-to-frame VIO update, as referred to as “Our w/o $f 2 f^{'}$ in Fig. 10 (c) and (d).

As can be seen, due to the LiDAR degeneration when facing a single plane, all three LiDAR-inertial odometry systems failed and did not return to the starting point. In contrast, by exploiting clues from the visual images, our proposed system works well in these two sequences and successfully returns to the starting point with drift down to 4.1 cm and 4.6 cm on sequences “degenerate_seq_00” and “degenerate_seq_01”, respectively. When the frame-to-frame VIO update is disabled, the estimated trajectory of “Our w/o f2f” appears to have sudden ”jumps” as the LiDAR data degrades (inside ellipse framebox of 10(c)), or even fail to return back to the origin (in Fig. 10(d)).

To obtain a more intuitive comprehension of the LiDAR degenerated scenarios, we recommend our readers to watch the accompanying video on YouTube: https://youtu.be/kZ8_ 7k3HpIk?t=370.

2) Evaluate of Robustness in Simultaneously LiDAR Degenerated and Visual Texture-Less Environments: In this experiment, we challenge one of the most difficult scenarios in SLAM, where both LiDAR and camera degenerate. We use sequence “degenerate_seq $_{-} 0 2^{\dag}$ of the R³LIVE-dataset, where the sensor device passes through a narrow “T”-shape passage (see Fig. 12) while occasionally facing against the side walls, causing LiDAR degeneration. Moreover, the visual texture on the white walls is very limited (Fig. 12(a) and Fig. 12(c)), especially for the wall-1, which has only changes in illumination. The absence of available LiDAR and visual features makes such scenarios rather challenging for both LiDAR-based and visual-based SLAM methods.

Taking advantage of the raw pixel color information and tightly fusing it with the LiDAR point cloud measurements, our proposed algorithm can “survive” in such extremely difficult scenarios. Fig. 13 shows our estimated pose, with the phases of passing through “wall-1” and “wall-2” shaded with purple and yellow, respectively. The estimated covariance is also shown in Fig. 13, which is bounded over the entire estimated trajectory, indicating that our estimation quality is stable over the entire process. The sensor is moved to the starting point, where an ArUco marker board is used to obtain the ground-true relative pose between the starting and end poses. Compared with the groundtrue end pose, our algorithm drifts 1.62^◦ in rotation and 4.57 cm in translation. We recommend the readers to the accompanying video on YouTube ( https://youtu.be/kZ8_7k3HpIk?t=440) for better visualization of the experiment.

Fig. 12. Tests in simultaneously LiDAR degenerated and visual texture-less environments.

F. Experiment-3: Evaluation of Radiance Map Reconstruction

In this experiment, we evaluate the accuracy of our proposed algorithm in reconstructing the radiance map. Since the ground-true radiance map of the environment can not be measured, we evaluate the accuracy based on two indicators: one is the estimation quality of the camera exposure time and the other is the average photometric error between the reconstructed radiance map and the measured images.

1) Evaluation of Exposure Time Estimation: In this experiment, we evaluate the accuracy ofthe estimated camera exposure time by comparing it with the ground-true value read from the camera’s API. We use four sequences (see Fig. 18) of the R³LIVE-dataset, where the data were collected by traveling through both interior and exterior of a complex building to ensure significant changes in lighting conditions. We compare our estimated results with Tum-cali [47] and DSO [25], which are two existing state-of-the-art baselines that can estimate the camera’s exposure time. All of these methods are initialized by assuming the exposure time of the first image frame is at a default value of 1ms.

Fig. 13. The estimated poses and their 3-σ bound with 5 times amplification for better visualization (the light-colored area around the trajectory) of the test in simultaneously LiDAR degenerated and visual texture-less environments. The shaded areas in purple and yellow are the phases of the sensors facing against the white “wall-1” and “wall-2”, respectively.

TABLE V
THE COMPARISON OF ESTIMATION ERROR OF EXPOSURE TIME OVER 5 SEQUENCES

Sequence	Our Mean/Max (ms)	Tum_cali Mean/Max (ms)	DSO Mean/Max (ms)
hku_campus_seq_02	3.460 / 20.311	7.082 / 36.175	7.605 / 43.194
hku_campus_seq_03	1.460 / 10.653	6.400 / 37.126	6.553 / 41.014
hku_main_building	2.572 / 16.855	5.196 / 26.775	5.550 / 30.281
hkust_campus_seq_02	0.189 / 1.185	0.341 / 1.451	2.428 / 17.719
hkust_campus_seq_03	0.302 / 3.514	5.225 / 13.361	0.381 / 2.040

The results are shown in Fig. 18, where the estimated exposure time of our method, Tum-cali [47], and DSO [25] are re-scaled to match with the ground-truth for better visualization. The average and maximum error of estimated exposure time w.r.t. the ground-truth is listed in Table V. As shown in Fig. 18 and Table V, our proposed method shows significantly lower estimation error than [47] and [25]. This is mainly because our method estimates the exposure time by minimizing the scan-to-map radiance error, while [47] and [25] recovers the exposure times from consecutive frames. The consequence is that our method can better utilize longer-term temporal intensity changes to restrain the drift of the exposure time estimation.

2) Evaluation ofRadiance Map: In this experiment, we evaluate the accuracy of our proposed algorithm in reconstructing the radiance map. Currently, LiDAR point cloud colorization remains one of the most challenging problems in the field of 3D reconstruction. The most common way is using the most recent image frame in time to give the color of each LiDAR frame [39], [49]. The preliminary implementation of our system R³LIVE published previously [45] colorized the point cloud by minimizing the photometric error similar to our current system but does not consider any exposure time estimation or photometric calibration. In this experiment, we compare our system against the previous implementation R³LIVE [45] to show the effectiveness of the exposure time estimation and photometric calibration and against the current baseline [39], [49] to show the advantage of the overall system.

TABLE VI
THE AVERAGE PHOTOMETRIC ERROR AMONG ALL SEQUENCES OF R³LIVE-DATASET

Sequence	Frames	baseline	R3LIVE	R³LIVE++
degenerate_seq_00	3315	30.58	21.36	16.19
degenerate_seq_01	1715	34.24	21.28	16.55
degenerate_seq_02	1694	27.14	20.30	15.97
hku_campus_seq_00	3016	34.78	22.56	14.57
hku_campus_seq_01	4502	34.97	22.47	16.30
hku_campus_seq_02	10409	39.01	23.73	16.85
hku_campus_seq_03	5557	42.95	24.78	16.93
hku_main_building	12157	43.12	22.26	19.04
hku_park_00	3410	43.29	25.17	19.02
hku_park_01	5251	43.86	27.01	20.07
hkust_campus_seq_00	16075	37.64	24.19	18.00
hkust_campus_seq_01	17426	39.09	24.67	18.53
hkust_campus_seq_02	15232	35.60	22.37	18.65
hkust_campus_seq_03	4031	41.74	23.77	18.92
Average	7413.57	38.60	23.58	18.01

To assess the radiance reconstruction error, after the map reconstruction, we re-project all points in the map with radiance information to each image frame with the estimated camera pose and calibrated photometric parameters. Then, we calculate the photometric error between the map point color and the RGB values of the image at the projected pixel location. The average photometric error of each image frame is calculated to evaluate the accuracy of the reconstructed radiance map. We perform the evaluation on all R³LIVE-dataset sequences, with the results of each sequence are given in Fig. 14, and the average photometric of each sequence is listed in Table VI. As can be seen, our system has consistently achieved the lowest photometric errors in all sequences and the next best is R³LIVE. Fig. 15 shows a few closeups of the reconstructed radiance map, from which we can clearly tell the words on objects. Moreover, in Fig. 16, we present the reconstructed radiance map of the sequence “hku_main_building” in R³LIVE-dataset, in which we collect the data in both interior and exterior of the main building of HKU. As shown in Fig. 16, both the indoor and outdoor details (e.g., marks on the road) are very clear, demonstrating that our proposed algorithm is of high accuracy. What worth mentioning is, this 3D radiance map is reconstructed on the fly as the data is being acquired (see the accompanying video on YouTube: https://youtu.be/kZ8_7k3HpIk?t=48). For more qualitative results of other sequences, we refer our readers to Section 3 of our Supplementary Material [54].

G. Run Time Analysis

In this section, we investigate the average time consumption of our proposed system on a CPU-Only PC (equipped with an Intel i7-9700K CPU and 64 GB RAM). Table VII listed the mean, maximum, and minimum of sequences’ average time consumption on both datasets (i.e., the NCLT-dataset and R³LIVE-dataset). For a detailed breakdown of the time consumption of each sequence, please refer to Section 3 of our Supplementary Material [54]. For the NCLT-dataset, each Li-DAR scan takes an average of 34.3 ms and each camera image takes an average of 16.6ms processing time. Since the data rate of the LiDAR and camera sensors are 10 Hz and 5 Hz, the total processing time per second is 426 ms, comprising of the time for processing 10 LiDAR scans (i.e., 343 ms) and 5 images (83 ms). For R³LIVE-dataset, each LiDAR scan takes an average of 22.7 ms and each camera image takes an average of 16.2 ms processing time. Since the data rate of the LiDAR and camera sensors are 10 Hz and 15 Hz, the total processing time per second is 470 ms, comprising ofthe time for processing 10 LiDAR scans (i.e., 235 ms) and 15 images (244 ms). In both cases, the processing time required per second is below half a second, indicating that our system runs in real-time (even two times faster than realtime) in both pose estimation and radiance map reconstruction. In addition, the processing time per image is similar across the two datasets while that for each LiDAR scan is quite different. The reason is that the LiDAR in R³LIVE-dataset has a much lower data rate than that of the NCLT-dataset (240k versus 695k points per second) while the cameras have similar resolution.

Fig. 14. Photometric errors between the reconstructed radiance map and image pixels.

Fig. 15. Closeup of a few scenes in the radiance map reconstructed by the baseline, R³LIVE and R³LIVE++.

Fig. 16. Our reconstructed radiance map of the main building of HKU. (a) The bird’s view of the map, with its details shown in $(b ~ n) .$ . (b ∼ g) closeup of outdoor scenarios and (h∼ n) closeup of indoor scenarios. To see the real-time reconstruction process of the map, please refer to the video on YouTube: https://youtu.be/kZ8_7k3HpIk?t=48.

Fig. 17. (a ∼ d) Images rendered from the reconstructed radiance map at different exposure time: 1 ms, 2 ms, 5 ms and 10 ms. (e) The HDR image merged from (a∼ d). Notice that for the sake of better visualization, those points that are overexposured are not displayed.

Fig. 18. Estimation of camera exposure time.

TABLE VII
THE MEAN, MAXIMUM, AND MINIMUM OF THE SEQUENCE’S AVERAGE TIME CONSUMPTION OF R³LIVE++ ON TWO DATASETS

NCLT-dataset	$M e a n \pm S T D$	$M a x i m u m \pm S T D$	Minimum ± STD
LiDAR frame (ms)	$\overset{―}{34.271 \pm 10.551}$	$\overset{―}{38.153 \pm 9.187}$	$29.070 \pm 8.807$
Camera frame (ms)	$16.600 \pm 4.168$	$17.613 \pm 4.275$	$15.090 \pm 4.164$
R³LIVE-dataset	$M e a n \pm S T D$	$M a x i m u m \pm S T D$	Minimum ± STD
LiDAR frame (ms)	$23.453 \pm 7.431$	$\overset{―}{30.757 \pm 5.109}$	$\overset{―}{8.111 \pm 3.622}$
Camera frame (ms)	$16.244 \pm 2.705$	$18.829 \pm 3.719$	$12.111 \pm 2.502$

VII. APPLICATIONS WITH R³LIVE

A. High Dynamic Range (HDR) Imaging

After the reconstruction of the radiance map, we are able to render an image by projecting the map to an image plane with a given pose and exposure time with (3). Taking the sequence “hku_main_building” as an example, Fig. 17(a), (b), (c) and (d) are the rendered images with global exposure time of 1 ms, 2 ms, 5 ms and 10 ms, respectively. These images rendered at different exposure times can be merged into an HDR image shown in Fig. 17(e).

B. R³LIVE++ for Various 3D Applications

While R³LIVE++ reconstructs the colored 3D map in realtime, we also develop software utilities to mesh and texture the reconstructed map offline, which are also publicly available on our GitHub repository [56]. With these developed utilities, we can export the reconstructed 3D maps to Unreal Engine [59] for enabling a series of 3D applications such as video games and vehicle simulators. For detailed showcases of our developed applications, we refer our reader to Section 4 of our Supplementary Material [54].

VIII. CONCLUSIONS AND FUTURE WORKS

A. Conclusion

In this paper, we proposed a novel LiDAR-inertial-visual fusion framework termed R³LIVE++ to achieve robust and accurate state estimation while simultaneously reconstructing the radiance map on the fly. This framework consists of two subsystems (i.e., the LIO and the VIO) that jointly and incrementally build a 3D radiance map of the environment in real-time. By tightly fusing the measurement of three different types of sensors, $R^{3} L I V E + +$ can achieve higher localization accuracy while being robust enough to scenarios with sensor degenerations.

In our experiments, we extensively validated our proposed algorithm with real-world experiments in terms of localization accuracy, robustness, and radiance map reconstruction accuracy. The benchmark results on 25 sequences from an open dataset (the NCLT-dataset) showed that $R^{3} L I V E + +$ achieved the highest overall accuracy among all other state-of-the-art SLAM systems under comparison. The evaluation on R3LIVE-dataset (self-collected) showed that the proposed framework was robust to extremely challenging scenarios that LiDAR and/or camera measurements degenerate (e.g., when the device is facing a single texture-less wall). Finally, compared with other counterparts, $R^{3} L I V E + +$ estimates the camera exposure time more accurately and reconstructs the true radiance information of the environment with significantly smaller errors when compared to the measured values in images.

To demonstrate the extendability of our work, we developed several applications based on our reconstructed radiance maps, such as high dynamic range (HDR) imaging, virtual environment exploration, and 3D video gaming. Finally, to share our findings and make contributions to the community, we made our codes, hardware design, and dataset publicly available on our GitHub repository [56].

B. Future Works

Due to the absence of a loop correction mechanism in $R^{3} L I V E + +$ , there is a possibility of gradual drift caused by the accumulation of localization errors when revisiting the same place. This can potentially result in inconsistent reconstructed outcomes upon revisits. In our forthcoming research, we aim to overcome this limitation by integrating our recent advancements in loop detection, as presented in [60], which is based on LiDAR point clouds. By employing this loop detection mechanism, we could identify loops in real-time and subsequently apply loop corrections to minimize drift and enhance the consistency of the reconstructed results.

Besides, the radiance map in $R^{3} L I V E + +$ is reconstructed with 3D points that contain radiance information, which prevents us from rendering high-resolution images from the radiance map due to the limited point cloud density of the radiance map (1 cm in our current implementation). While further increasing this point cloud density is possible, it will further increase the processing time. This point density is also limited by the density of the raw points measured by LiDAR sensors. Noticing that images often have much higher resolution, in the future, we could explore how to make full use of such high-resolution images in the fusion framework.

ACKNOWLEDGMENTS

The authors would like to thank DJI Company, Ltd.¹ for providing devices and research funds.

REFERENCES

[1] F. Gao, W. Wu, W. Gao, and S. Shen, “Flying on point clouds: Online trajectory generation and autonomous navigation for quadrotors in cluttered environments,” J. Field Robot., vol. 36, no. 4, pp. 710–733, 2019.

[2] F. Kong, W. Xu, Y. Cai, and F. Zhang, “Avoiding dynamic small obstacles with onboard sensing and computation on aerial robots,” IEEE Robot. Automat. Lett., vol. 6, no. 4, pp. 7869–7876, Oct. 2021.

[3] H. Lategahn, A. Geiger, and B. Kitt, “Visual SLAM for autonomous ground vehicles,” in Proc. IEEE Int. Conf. Robot. Automat., 2011, pp. 1732–1737.

[4] P. Beinschob and C. Reinke, “Graph slam based mapping for AGV localization in large-scale warehouses,” in Proc. IEEE Int. Conf. Intell. Comput. Commun. Process., 2015, pp. 245–248.

[5] J. Lin, X. Liu, and F. Zhang, “A decentralized framework for simultaneous calibration, localization and mapping with multiple LiDARs,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2020, pp. 4870–4877.

[6] J. Levinson et al., “Towards fully autonomous driving: Systems and algorithms,” in Proc. IEEE Intell. Veh. Symp., 2011, pp. 163–168.

[7] G. Ros, A. Sappa, D. Ponsa, and A. M. Lopez, “Visual slam for driverless cars: A brief survey,” in Proc. Intell. Veh. Symp. Workshops, 2012, pp. 1–6.

[8] A. Singandhupe and H. M. La, “A review of slam techniques and security in autonomous driving,” in Proc. 3rd IEEE Int. Conf. Robotic Comput., 2019, pp. 602–607.

[9] O. Pink, “Visual map matching and localization using a global feature map,” in Proc. IEEE Comput. Soc. Conf. Comput. Vis. Pattern Recognit. Workshops, 2008, pp. 1–7.

[10] M. Bürki, M. Dymczyk, I. Gilitschenski, C. Cadena, R. Siegwart, and J. Nieto, “Map management for efficient long-term visual localization in outdoor environments,” in Proc. IEEE Intell. Veh. Symp., 2018, pp. 682– 688.

[11] B. Nagy and C. Benedek, “Real-time point cloud alignment for vehicle localization in a high resolution 3D map,” in Proc. Eur. Conf. Comput. Vis. Workshops, 2018, pp. 226–239.

[12] W. Li et al., “AADS: Augmented autonomous driving simulation using data-driven algorithms,” Sci. Robot., vol. 4, no. 28, 2019, Art. no. eaaw0863.

[13] Z. Bao, S. Hossain, H. Lang, and X. Lin, “A review of high-definition map creation methods for autonomous driving,” Eng. Appl. Artif. Intell., vol. 122, 2023, Art. no. 106125.

[14] C. Cadena et al., “Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age,” IEEE Trans. Robot., vol. 32, no. 6, pp. 1309–1332, Dec. 2016.

[15] J. Lin and F. Zhang, “Loam_livox: A fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small FoV,” in Proc. IEEE Int. Conf. Robot. Automat., 2020, pp. 3126–3131.

[16] J. Zhang and S. Singh, “LOAM: Lidar odometry and mapping in real-time,” in Proc. Robot.: Sci. Syst. Conf., vol. 2, no. 9, 2014, pp. 1–9.

[17] T. Shan and B. Englot, “LeGO-LOAM: Lightweight and ground-optimized LiDAR odometry and mapping on variable terrain,” in Proc. IEEE/RSJInt. Conf. Intell. Robots Syst., 2018, pp. 4758–4765.

[18] C. Park, P. Moghadam, J. L. Williams, S. Kim, S. Sridharan, and C. Fookes, “Elasticity meets continuous-time: Map-centric dense 3D LiDAR SLAM,” IEEE Trans. Robot., vol. 38, no. 2, pp. 978–997, Apr. 2022.

[19] H. Ye, Y. Chen, and M. Liu, “Tightly coupled 3D LiDAR inertial odometry and mapping,” in Proc. Int. Conf. Robot. Automat., 2019, pp. 3144–3150.

[20] T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, “LIO-SAM: Tightly-coupled LiDAR inertial odometry via smoothing and mapping,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2020, pp. 5135–5142.

[21] K. Li, M. Li, and U. D. Hanebeck, “Towards high-performance solidstate-LiDAR-inertial odometry and mapping,” IEEE Robot. Automat. Lett., vol. 6, no. 3, pp. 5167–5174, Jul. 2021.

[22] C. Qin, H. Ye, C. E. Pranata, J. Han, S. Zhang, and M. Liu, “LINS: A lidar-inertial state estimator for robust and efficient navigation,” in :Proc. IEEE Int. Conf. Robot. Automat., 2020, pp. 8899–8906.

[23] W. Xu and F. Zhang, “FAST-LIO: A fast, robust LiDAR-inertial odometry package by tightly-coupled iterated kalman filter,” IEEE Robot. Automat. Lett., vol. 6, no. 2, pp. 3317–3324, Apr. 2021.

[24] W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, “FAST-LIO2: Fast direct LiDAR-inertial odometry,” IEEE Trans. Robot., vol. 38, no. 4, pp. 2053– 2073, Aug. 2022.

[25] J. Engel, V. Koltun, and D. Cremers, “Direct sparse odometry,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 40, no. 3, pp. 611–625, Mar. 2018.

[26] A. J. Davison, I. D. Reid, N. D. Molton, and O. Stasse, “MonoSLAM: Real-time single camera SLAM,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 29, no. 6, pp. 1052–1067, Jun. 2007.

[27] G. Klein and D. Murray, “Parallel tracking and mapping for small ar workspaces,” in Proc. 6th IEEE ACM Int. Symp. Mixed Augmented Reality, 2007, pp. 225–234.

[28] R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos, “ORB-SLAM: A versatile and accurate monocular slam system,” IEEE Trans. Robot., vol. 31, no. 5, pp. 1147–1163, Oct. 2015.

[29] R. Mur-Artal and J. D. Tardós, “ORB-SLAM2: An open-source slam system for monocular, stereo, and RGB-D cameras,” IEEE Trans. Robot., vol. 33, no. 5, pp. 1255–1262, Oct. 2017.

[30] T. Qin, P. Li, and S. Shen, “VINS-Mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Trans. Robot., vol. 34, no. 4, pp. 1004–1020, Aug. 2018.

[31] A. Rosinol, M. Abate, Y. Chang, and L. Carlone, “Kimera: An open-source library for real-time metric-semantic localization and mapping,” in Proc. IEEE Int. Conf. Robot. Automat., 2020, pp. 1689–1696.

[32] 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 Trans. Robot., vol. 37, no. 6, pp. 1874–1890, Dec. 2021.

[33] B. D. Lucas et al., “An iterative image registration technique with an application to stereo vision,” in Proc. 7th Int. Joint Conf. Artif. Intell., Vancouver, 1981, pp. 674–679.

[34] J. Engel, T. Schöps, and D. Cremers, “LSD-SLAM: Large-scale direct monocular SLAM,” in Proc. Eur. Conf. Comput. Vis., Springer, 2014, pp. 834–849.

[35] C. Forster, M. Pizzoli, and D. Scaramuzza, “SVO: Fast semi-direct monocular visual odometry,” in Proc. IEEE Int. Conf. Robot. Automat., 2014, pp. 15–22.

[36] J. Zhang and S. Singh, “Laser–visual–inertial odometry and mapping with high robustness and low drift,” J. Field Robot., vol. 35, no. 8, pp. 1242– 1264, 2018.

[37] W. Shao, S. Vijayarangan, C. Li, and G. Kantor, “Stereo visual inertial LiDAR simultaneous localization and mapping,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2019, pp. 370–377.

[38] T. Laidlow, M. Bloesch, W. Li, and S. Leutenegger, “Dense RGB-D-inertial slam with map deformations,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2017, pp. 6741–6748.

[39] Y. Zhu, C. Zheng, C. Yuan, X. Huang, and X. Hong, “CamVox: A low-cost and accurate lidar-assisted visual SLAM system,” in Proc. IEEE Int. Conf. Robot. Automat., 2021, pp. 5049–5055.

[40] X. Zuo, P. Geneva, W. Lee, Y. Liu, and G. Huang, “LIC-Fusion: LiDARinertial-camera odometry,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2019, pp. 5848–5854.

[41] X. Zuo, Y. Yang, J. Lv, Y. Liu, G. Huang, and M. Pollefeys, “Lic-fusion 2.0: Lidar-inertial-camera odometry with sliding-window plane-feature tracking,” in Proc. Int. Conf. Intell. Robots Syst., 2020, pp. 5112–5119.

[42] D. Wisth, M. Camurri, and M. Fallon, “VILENS: Visual, inertial, lidar, and leg odometry for all-terrain legged robots,” IEEE Trans. Robot., vol. 39, no. 1, pp. 309–326, Feb. 2023.

[43] T. Shan, B. Englot, C. Ratti, and D. Rus, “LVI-SAM: Tightly-coupled lidar-visual-inertial odometry via smoothing and mapping,” in Proc. IEEE Int. Conf. Robot. Automat., 2021, pp. 5692–5698.

[44] J. Lin, C. Zheng, W. Xu, and F. Zhang, “R²LIVE: A robust, real-time, LiDAR-inertial-visual tightly-coupled state estimator and mapping,” IEEE Robot. Automat. Lett., vol. 6, no. 4, pp. 7469–7476, Oct. 2021.

[45] J. Lin and F. Zhang, “R³LIVE: A robust, real-time, RGB-colored, LiDARinertial-visual tightly-coupled state estimation and mapping package,” in Proc. Int. Conf. Robot. Automat., 2022, pp. 10672–10678.

[46] D. He, W. Xu, and F. Zhang, “Symbolic representation and toolkit development of iterated error-state extended kalman filters on manifolds,” IEEE Trans. Ind. Electron., vol. 70, no. 12, pp. 12533–12544, Dec. 2023.

[47] P. Bergmann, R. Wang, and D. Cremers, “Online photometric calibration of auto exposure video for realtime visual odometry and SLAM,” IEEE Robot. Automat. Lett., vol. 3, no. 2, pp. 627–634, Apr. 2018.

[48] A. Segal, D. Haehnel, and S. Thrun, “Generalized-ICP,” in Proc. Robot.: Sci. Syst., Seattle, WA, 2009, Art. no. 435.

[49] C. Zheng, Q. Zhu, W. Xu, X. Liu, Q. Guo, and F. Zhang, “FAST-LIVO: Fast and tightly-coupled sparse-direct LiDAR-inertial-visual odometry,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2022, pp. 4003–4009.

[50] R. Wang, M. Schworer, and D. Cremers, “Stereo DSO: Large-scale direct sparse visual odometry with stereo cameras,” in Proc. IEEE Int. Conf. Comput. Vis., 2017, pp. 3903–3911.

[51] T. Qin and S. Shen, “Online temporal calibration for monocular visualinertial systems,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2018, pp. 3662–3669.

[52] G. Grimmett and D. Stirzaker, Probability and Random Processes. London, U.K.: Oxford Univ. Press, 2020.

[53] N. Carlevaris-Bianco, A. K. Ushani, and R. M. Eustice, “University of michigan north campus long-term vision and lidar dataset,” Int. J. Robot. Res., vol. 35, no. 9, pp. 1023–1035, 2016.

[54] J. Lin and F. Zhang, “Supplementary material for R³LIVE,” 2024. [Online]. Available: https://github.com/ziv-lin/r3live_dataset/raw/main/ supply/r3live_plus_plus_supplementary_material.pdf

[55] S. Garrido-Jurado, R. Muñoz-Salinas, F. J. Madrid-Cuevas, and M. J. Marín-Jiménez, “Automatic generation and detection of highly reliable fiducial markers under occlusion,” Pattern Recognit., vol. 47, no. 6, pp. 2280–2292, 2014.

[56] J. Lin and F. Zhang, “Github repository for R³LIVE,” 2024. [Online]. Available: https://github.com/hku-mars/r3live

[57] Z. Zhang and D. Scaramuzza, “A tutorial on quantitative trajectory evaluation for visual (-inertial) odometry,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2018, pp. 7244–7251.

[58] W. Sun, Y. Hu, D. G. MacDonnell, C. Weimer, and R. R. Baize, “Technique to separate lidar signal and sunlight,” Opt. Exp., vol. 24, no. 12, pp. 12949– 12954, 2016.

[59] Epic Games, “Unreal Engine,” 1998. [Online]. Available: https://www. unrealengine.com

[60] C. Yuan, J. Lin, Z. Zou, X. Hong, and F. Zhang, “STD: Stable triangle descriptor for 3D place recognition,” in Proc. IEEE Int. Conf. Robot. Automat., 2023, pp. 1897–1903.

Jiarong Lin (Member, IEEE) received the BS degree in optical information science and technology from the University of Electronic Science and Technology of China (UESTC), in 2015. He is currently working toward the PhD degree with the Department of Mechanical Engineering, the University of Hong Kong (HKU), Hong Kong, China. His research interests include light detection and ranging (LiDAR) mapping and sensor fusion.

Fu Zhang (Member, IEEE) received the BE degree in automation from the University of Science and Technology of China (USTC), Hefei, Anhui, China, in 2011, and the PhD degree in controls from the University of California at Berkeley, Berkeley, CA, USA, in 2015. He joined the Department of Mechanical Engineering, The University of Hong Kong (HKU), Hong Kong, as an assistant professor, in August 2018. His current research interests are on robotics and controls, with a focus on unmanned aerial vehicle (UAV) design, navigation, control, and light detection and ranging (LiDAR)-based simultaneous localization and mapping (SLAM).

本页由 md/2024_R___3__3LIVE____A_Robust__Real-Time__Radiance_Reconstruc.md 预渲染生成。公式以 MathML 呈现，浏览器原生渲染，不依赖外部脚本或字体 CDN。

R3LIVE++: A Robust, Real-Time, Radiance Reconstruction Package With a Tightly-Coupled LiDAR-Inertial-Visual State Estimator