LVBA: LiDAR-Visual Bundle Adjustment for RGB Point Cloud Mapping

Our visual BA begins by generating local scene points for each image frame. When selecting local scene points, we prefer points surrounded by complex textures as they provide richer photometric details and effective constraints when projected onto an image frame. Specifically, for each camera frame, its image is divided into grid cells. The LiDAR planar feature points captured when the position of LiDAR is close to this camera frame are then projected onto these grid cells. To ensure the view quality of the scene point, only those LiDAR feature points whose surface normal is alongside the view direction from the camera frame are taken into consideration. Specifically, the projected LiDAR feature points should fulfill the following condition:

where $\mathbf{p}_{f}$ and $\mathbf{n}_{f}$ are the position and normal vector of the LiDAR planner feature point, respectively. $\mathbf{t}_{C}$ is the position of the camera frame, and $\alpha_{0}$ is a threshold. After each LiDAR feature point is projected, a score reflecting the intensity gradients around the candidate scene point is subsequently computed for every projected point by employing the Difference of Gaussian (DoG) method [20]. Within each grid cell, the point with the highest score is then selected as a local scene point. These selected local scene points are denoted as $\boldsymbol{\pi}=(\mathbf{p},\mathbf{n})\in\mathbb{R}^{3}\times\mathbb{S}^{2}$, where $\mathbf{p}$ represents the position of the point and $\mathbf{n}$ represents the normal vector estimated by our LiDAR BA. For each selected local scene point, we denote the corresponding camera frame as the “reference frame” of this local scene point. In scenarios where a grid cell only contains points with low scores (indicating a lack of useful photometric information, such as in a textureless surface), no points are selected from that grid cell.

After generating local scene points for each frame, our next step is to identify the other frames that can observe these points. These identified frames are then denoted as the “target frame” of each scene point and will be then used to construct the corresponding cost item in (II-C4). To facilitate this, a local visibility determination process is implemented. In the process, only frames within a sliding window relative to the reference frame of a local scene point are considered. Since these frames are close to the position of the reference frame, hence the parallax is insignificant and a local scene point is usually directly visible by these frames without further occlusion check. As a consequence, we only have to examine if the view direction between the local scene point $\boldsymbol{\pi}=(\mathbf{p},\mathbf{n})$ and the candidate target frame at $\mathbf{T}_{t}=(\mathbf{R}_{t},\mathbf{t}_{t})$ is within the frame’s FoV. The FoV check is much more efficient than the occlusion check based on ray-casting but may give false visibility results for scene points on the edge of foreground and background objects. To fix this issue, we project, using the initial sensor pose, the local scene point onto the reference frame, and the candidate target frame to evaluate the photometric discrepancy between them. If a scene point is truly visible in the target frame, the initial photometric discrepancies are often small. Finally, to ensure the view quality of the scene point, we choose target frames that are viewing the scene point along a direction close to the point surface normal. Consequently, the local visibility determination checks are:

where $\mathbf{d}=\frac{\mathbf{p}-\mathbf{t}_{t}}{\|\mathbf{p}-\mathbf{t}_{t}\|_{2}}$ denotes the view direction from the candidate target frame to the local scene point, $\mathbf{z}_{C}$ express the z-axis of the camera frame (vertical to the camera image plane), $\mathbf{I}_{r}$ and $\mathbf{I}_{t}$ are the RGB values of patches generated on the reference frame and the candidate target frame, respectively, and $\mathtt{NCC}(\cdot,\cdot)$ is the Normalized Cross-Correlation (NCC) [21] between the two patches. The details for patch generation are illustrated in Sec. 4. $\alpha_{1}$, $\alpha_{2}$, and $\alpha_{3}$ are three thresholds. If a local scene point is deemed visible by this frame (i.e., satisfying (3)), it will be selected as a target frame and contribute to the final optimization cost in (II-C4).

Local scene points provide effective local constraints for camera pose optimization. However, they fail to provide more global constraints for camera poses far apart observing the same area. Therefore, we introduce global scene points and global visibility that provide more comprehensive, global constraints. When generating global scene points and determining global visibility, the distance between the reference and target frames increases, making the occlusion check necessary. Typically, a ray-casting algorithm (e.g., in [22]) is employed to address the occlusion check, but this approach not only increases the computation costs but also risks inaccuracies in sparser point cloud scenarios. To overcome these challenges, we implement a LiDAR-assisted method based on a global visibility voxel map, addressing the occlusion check with more reliable results and reduced computational load, as illustrated in Fig. 3.

The visual BA in LVBA optimizes the photometric discrepancy of a scene point $\boldsymbol{\pi}=(\mathbf{p},\mathbf{n})\in\mathbb{R}^{3}\times\mathbb{S}^{2}$ (either local or global scene points) between the reference frame and target frame, whose poses are denoted as $\mathbf{T}_{r}=(\mathbf{R}_{r},\mathbf{t}_{r})\in SE(3)$ and $\mathbf{T}_{t}=(\mathbf{R}_{t},\mathbf{t}_{t})\in SE(3)$, respectively. As shown in Fig. 4, a cost item is constructed below:

Firstly, the scene point $\boldsymbol{\pi}$ is projected to the reference image plane via the pin-hole camera projection model:

where $\mathbf{K}$ is the camera intrinsic matrix, $\mathbf{\hat{u}}_{r}\in\mathbb{R}^{3}$ is the homogeneous format of $\mathbf{u}_{r}\in\mathbb{R}^{2}$ in the reference image plane coordinate (without further notice, we use $\hat{(\cdot)}$ to denote the homogeneous format of a vector). Then, a reference patch $\{\mathbf{u}_{r}^{(i)}\}$ is generated around the projected point. Utilizing the plane normal of the scene point and the camera projection model, the reference patch can be projected to the target frame via a homography transformation[23]:

where the homography matrix $\mathbf{H}$ can be expressed as

and $\mathbf{E}$ is $3\times 3$ identity matrix. Finally, the photometric cost is constructed as:

where $\mathbf{I}_{r}(\boldsymbol{\cdot})$ and $\mathbf{I}_{t}(\boldsymbol{\cdot})$ calculate RGB color vector by bi-linear interpolation on the reference image and target image, respectively, $\boldsymbol{\mathcal{T}}_{r}=(\mathbf{T}_{r},\epsilon_{r})$ and $\boldsymbol{\mathcal{T}}_{t}=(\mathbf{T}_{t},\epsilon_{t})$ represent the camera states of reference and target frames, $\epsilon_{r}$ and $\epsilon_{t}$ represent the relative exposure time of them and $\boldsymbol{\Sigma}$ is the covariance matrix of the cost function. The relative exposure time $\epsilon$ and covariance matrix $\boldsymbol{\Sigma}$ are defined using a simplified camera measurement model. Further details can be referred to the APPENDIX A¹¹1https://github.com/hku-mars/mapping_eval.

With our constructed photometric cost item, the optimal estimation of camera states $\boldsymbol{\mathcal{T}}^{\ast}$ is given by:

where $\boldsymbol{\mathcal{T}}=\{\boldsymbol{\mathcal{T}}_{1},\boldsymbol{\mathcal{T}}_{2},\cdots,\boldsymbol{\mathcal{T}}_{M_{p}}\}$ is the camera state set, $\boldsymbol{\Pi}=\{\boldsymbol{\pi}_{1},\boldsymbol{\pi}_{2},\cdots,\boldsymbol{\pi}_{M_{s}}\}$ is the scene point set (both local and global), $M_{p}$ and $M_{s}$ represents the total number of camera poses and scene points, respectively, $r(i)$ represents the reference frame index of the $i$-th scene point, $V_{i}$ is the target frame set (both local and global), and $L_{\text{photo}}$ is the photometric cost item, as defined in (II-C4). In LVBA, we employed a Levenberg-Marquardt optimizer to minimize the cost function in (9).

To enhance the robustness of the cost function against initial estimation, our visual BA employs an iterative coarse-to-fine strategy. This strategy involves the process of down-sampling the image and sequentially optimizing the camera states from the top layer of the pyramid to the original resolution. Within each iteration, we utilize the optimized states obtained from the previous higher layer to generate scene points and determine visibility.

To evaluate the mapping accuracy and consistency with our optimization results, we use a colorizing-and-rendering algorithm. Initially, an RGB-radiance point cloud is created using LiDAR scans and camera images. For each LiDAR point $\mathbf{p}^{L}$ in the LiDAR frame $\mathbf{T}_{L}=(\mathbf{R}_{L},\mathbf{t}_{L})$, the nearest camera frame at $\mathbf{T}_{C}=(\mathbf{R}_{C},\mathbf{t}_{C})$ is identified. The radiance value of this point is calculated as:

where $\epsilon$ is the estimated relative exposure time of the camera frame, for benchmark methods that didn’t estimate the relative exposure time, $\epsilon$ is set to $1$, $\boldsymbol{\Pi}(\cdot)$ denotes the projection process to the camera’s image plane, and $\mathbf{I}(\cdot)$ yields the RGB value. After constructing the radiance map, we render raw images from the radiance map. For each image with optimized camera frame pose and relative exposure time $\epsilon$, we project the colorized radiance map onto its image plane to create a rendered image, the RGB color of the image is determined by $\mathbf{I}=\epsilon\mathbf{r}$. An example of our rendering result is shown in Fig. 5. Finally, the discrepancy between these rendered images and the original raw images is evaluated using Peak Signal-to-Noise Ratio (PSNR) and Structural Similarity Index Measure (SSIM) metrics. The source code of our evaluation toolchain is available on GitHub²²2https://github.com/hku-mars/mapping_eval.

In this section, we performed a mapping accuracy evaluation among all three datasets. Due to the strict time synchronization requirement of FAST-LIVO, which is not satisfied in the R³LIVE dataset, we didn’t evaluate FAST-LIVO with the R³LIVE dataset. Further, as LVBA and Colmap-PCD only estimate the poses of the keyframes, we assessed R³LIVE and FAST-LIVO only at those keyframes, although their results are obtained by running on all frames at 10Hz.

To assess the effectiveness of our proposed global scene point selection and their visibility determination, we ran our algorithm without utilizing any global scene point constraints (termed “w/o GSP”). Furthermore, to evaluate the effectiveness of our proposed relative exposure time estimation, we ran the algorithm by assuming all relative exposure time to be one (termed “w/o RET”). Both experiments were conducted under the same settings of our full method as illustrated above.

The evaluation results are computed and reported in TABLE I. Compared with two LiDAR-visual-inertial odometry (i.e. R³LIVE and FAST-LIVO), our LVBA (Full) demonstrates significant improvements in mapping accuracy across all tested sequences. Both R³LIVE and FAST-LIVO are based on the ESIKF framework and lack the ability to effectively correct historical errors. When colorizing the point cloud with all images, those image frames with substantial state errors are revealed by the color blur of point clouds, especially when the sequences get longer. In contrast, our proposed global LiDAR-visual BA could optimize the state estimation of all image frames, leading to a global consistency and low color blur, as illustrated in Fig. 6. Compared with Colmap-PCD, our LVBA (Full) achieved better performance on most of the sequences, except the SSIM on hku2 in the FAST-LIVO dataset. Since the sequence hku2 is captured in a night scenario, the image is taken with a high camera gain, resulting in increased measurement noise. This noise can potentially impact our optimization process. In all the rest sequences, Colmap-PCD is affected by varying degrees of errors introduced by feature extraction, matching, and point-to-plane association, resulting in lower performance. Additionally, for those sequences with significant exposure time variance, Colmap-PCD may yield inferior results compared to our method due to the lack of exposure time estimation (e.g. sequence visual challenge), or even complete failure due to the difficulty in identifying the proper feature matching (e.g. sequence hku campus seq 01). Furthermore, when evaluating on the R³LIVE dataset, we observed that Colmap-PCD encountered challenges with system initialization, leading to system failures or inaccurate pose estimations. Some qualitative mapping results are illustrated in Fig. 6, demonstrating the better resolution and consistency of our method in constructing high-fidelity colorized 3D maps. More visualization results can be found in our video at https://youtu.be/jtIUBI0U76c.

Compared with the LVBA without global scene point constraints (“w/o GSP”) or relative exposure time estimation (“w/o RET”), our full algorithm, incorporating global constraints, significantly improves mapping quality. Fig. 7 illustrates that a lack of global constraints provided by our global scene points results in color blurring and separation in the colorized point cloud map, and failing to estimate relative exposure times leads to noticeable brightness and color discrepancies between the original image and our rendered result. The sequences hku campus seq 02/03, which provide the ground truth for camera exposure time, were used to compare our estimated relative exposure times against the actual values. As depicted in Fig. 8, while our estimates do not provide the absolute values due to the simplified model, they still successfully capture the general trend of the ground truth exposure times.