{'fraction_non_alphanumeric': 0.03146545401202875, 'fraction_numerical': 0.016246149332550974, 'mean_word_length': 4.6445870420202855, 'pattern_counts': {'":': 0, '<': 4, '<?xml version=': 0, '>': 1, 'https://': 1, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Monocular Camera Localization in Prior LiDAR Maps with 2D-3D Line Correspondences

Light-weight camera localization in existing maps is essential for vision-based navigation. Currently, visual and visual-inertial odometry (VO\&VIO) techniques are well-developed for state estimation but with inevitable accumulated drifts and pose jumps upon loop closure. To overcome these problems, we propose an efficient monocular camera localization method in prior LiDAR maps using directly estimated 2D-3D line correspondences. To handle the appearance differences and modality gaps between untextured point clouds and images, geometric 3D lines are extracted offline from LiDAR maps while robust 2D lines are extracted online from video sequences. With the pose prediction from VIO, we can efficiently obtain coarse 2D-3D line correspondences. After that, the camera poses and 2D-3D correspondences are iteratively optimized by minimizing the projection error of correspondences and rejecting outliers. The experiment results on the EurocMav dataset and our collected dataset demonstrate that the proposed method can efficiently estimate camera poses without accumulated drifts or pose jumps in urban environments. The code and our collected data are available at https://github.com/levenberg/2D-3D-pose-tracking.


I. INTRODUCTION
Accurate robot localization in urban environments is in great demand for autonomous vehicles. Since GPS localization is unstable without direct line-of-sight to the satellites, LiDAR-based localization modules are often used because of the accurate range measurements. Additionally, urban scenes have relative time-invariant geometric structures (e.g., buildings), thus one-time 3D map construction can be used for long-term localization. However, the expensive cost and heavy weight of LiDAR sensors limit its wide applications. Cameras and IMUs are low-cost, light-weight and commonly available sensors and current visual-inertial based pose estimation and mapping methods are well-developed for a variety of robot systems [1], [2], [3]. Nevertheless, state estimation methods that use only image features are prone to failures due to lighting or texture changes in the environment. Consequently, if camera localization modules can be associated with a prior 3D map, i.e., fuse the visual information with range measurements, there will be a great potential to use these light-weight and small camera modules for accurate localization in urban environments without a LiDAR.
However, the fusion of image data with 3D point clouds is challenging due to appearance differences and modality  gaps. Current approaches typically transfer the 3D data into 2D space or reconstruct 3D point clouds from 2D images to align data for pose estimation. Based on the characteristics of urban environments, our intuition lies on the fact that the major geometric structures, such as lines and planes, can be both captured in 3D maps and 2D images regardless of appearance differences and modality gaps. The direct 2D-3D geometric co-occurrence correspondence is more robust and precise than the association of domain-transferred data. Therefore, our purpose is to directly estimate the 2D-3D geometric line correspondences for the accurate and longterm camera localization.
In this work, we propose an approach for real-time lightweight monocular camera localization in prior 3D LiDAR maps using direct 2D-3D geometric line correspondences. We assume a coarse pose initialization is given and focus on the pose tracking in maps, which follows the related works [4], [5]. For geometric concurrent feature extraction, 3D line segments are detected offline from LiDAR maps while robust 2D line segments are extracted online from video sequences. By employing the 6-DOF pose prediction from VIO, local visible 3D lines in field-of-view (FoV) are extracted and directly matched with 2D line features to obtain coarse 2D-3D line correspondences. Finally, the camera pose and 2D-3D correspondences are iteratively optimized by minimizing the projection error of correspondences and rejecting outliers.
The main contribution of this work is to estimate geometric 2D-3D line correspondences for camera localization, which efficiently associates every keyframe with the prior LiDAR map. The 2D-3D line correspondence estimation is robust to appearance changes and suitable for camera localization in urban environments. Fig. 1 shows a camera image with 2D-3D line correspondences and estimated camera poses in the LiDAR map.

II. RELATED WORK
Vision-based localization in maps is to establish correspondences between 2D and 3D modalities for improving the localization robustness and accuracy [6]. To overcome the modality gaps and appearance differences, general approaches are using "intermediate products" to transfer the matching into the same space, i.e., in 2D space [7] or in 3D space [4], [8].
The first kind of camera localization in 3D maps is matching photometry in 2D image space. For most of the visual simultaneous localization and mapping (SLAM) methods [1], [2], sparse point cloud maps are reconstructed with photometry (or visual) descriptors, then loop closure detection is conducted by matching these descriptors to decrease drifts. In image-based localization methods [7], [9], large scale maps are reconstructed using SFM with visual features, these features are efficiently matched to yield 2D-3D correspondences for camera localization. Nevertheless, appearance and visual features are sensitive to illumination changes and light conditions, which make the correspondences unstable for longterm camera localization. Additionally, to handle the LiDAR maps without associated visual features, LiDAR appearance synthetic images are often used to directly match with camera images by normalized mutual information (NMI) [10] or normalized information distance (NID) [11]. Additionally, recent methods utilize colored point cloud projections [12] and synthesized depth images [13] to match with live images for camera localization. These localization methods are all trying to transfer the 2D-3D matching problem to be a 2D-2D matching problem.
The second strategy is matching geometry in 3D space to estimate camera poses. By using local bundle adjustment, [4] proposed to match a sparse reconstructed local 3D point clouds with given 3D LiDAR maps, which solves the scale estimation problem of monocular VO system and achieves online estimation of 6-DoF camera poses. Similarly in [14], 3D structural descriptors are used for matching LiDAR maps with sparse visual reconstructed point clouds. In [8], [5], dense local point clouds are reconstructed from a stereo camera to match with the 3D LiDAR maps, and then the matching results are loosely or tightly coupled into the VO and VIO system for optimizing camera poses. These localization methods by 3D registration obtain feasible results compared with vision-only based methods. However, the localization accuracy highly depends on local reconstruction performances. SFM suffers from scale problem and the reconstructed sparse points may not have correspondence in maps. Stereo reconstruction gives dense local point clouds but mostly is time-consuming and does not scale well for long-range depth estimation.
Compared with the local point cloud matching methods, we aim at directly extracting 2D lines from a monocular camera to match with 3D lines from LiDAR maps for camera localization, which does not rely on SFM or stereo reconstruction modules. The global 2D-3D registration is known as a kidnapped robot problem for the feature description gap and the non-convexity [15]. RANSAC-based and branchand-bound strategies are often used to maximize the global optimum without a pose prior [15], [16]. With the camera pose prediction from GPS or VO (VIO), we can obtain a pose prediction as the prior for local 2D-3D matching. SoftPosit [17] and BlindPnP [18] are conventional simultaneous 2D-3D correspondences and pose estimation approaches with the provided pose initialization. For the camera to LiDAR sensor calibration methods [19], [20], geometric feature correspondences across images and point clouds are often used to optimize the extrinsic parameters. Recently, some learning-based methods were proposed to directly register images to LiDAR-Map with GPS pose initialization [21], [22]. These methods suffer from either high computation complexity or unstable outputs, which still need exploration for realtime camera localization in 3D LiDAR maps.

III. PROPOSED METHOD
The proposed method simultaneously estimates 6-DoF camera poses and 2D-3D line correspondences in LiDAR maps. 2D-3D line correspondences are utilized to optimize camera poses by minimizing the 3D line projection error while refined camera poses can help reject outlier correspondences. As the preliminary to the online 2D-3D correspondence estimation, 3D line features are extracted offline on the large scale 3D LiDAR maps. At the same time, a coarse pose initialization is given for the first frame by the PnP solver [23] on manually labeled 2D-3D point correspondences. Then VINS-Mono [2] is utilized to predict the camera motion between adjacent keyframes. With the predicted poses, local 3D lines in camera field-of-view (FoV) are extracted and directly matched with the online extracted 2D lines from image sequences. Finally, camera poses and 2D-3D correspondences are iteratively updated. The pipeline is shown in Fig. 2.

A. 2D and 3D line extraction
In urban environments, the geometric structures are often represented by line segments and planes. Here we utilize a simple segmentation-based 3D line detection method [24] to extract 3D lines from the LiDAR maps. The general idea is to cluster point clouds into plane regions and use contour line fitting to obtain 3D line segments. This method is efficient and robust for large scale unorganized point clouds in urban environments. Although it needs time to process millions of points, the 3D lines of all maps are extracted only once before we start tracking the camera poses in the maps.
For 2D line extraction, we want to extract the major geometric 2D lines which are consistent with 3D lines and robust to noises. It is challenging in urban scenes because the substantial texture noises yield fragmented 2D line segments and some geometric edges are invisible in 2D images on the color-homogeneous structures (e.g., white wall). Many state-of-the-art line segment detection (LSD) methods have been proposed in computer vision [25], [26], where traditional hand-craft methods are with high efficiency for online running on CPU. However, the detected lines are fragmented and noisy, an example is shown in Fig. 3(a). These kinds of fragmented and noisy features can produce a lot of 2D-3D matching outliers. Considering the line completeness and robustness to noisy, we finally employ a learning-based LSD [26] which uses the attraction field map (AFM) to transfer the LSD problem to be a region coloring problem. For each pixel p in images, the model first learns a 2D vector a(p) from the pixel to the nearest point p on the nearest line segment.
where p = (x, y) is the coordinate of pixel p. Then an AFM {a(p), p ∈ I} is generated by encoding the pixels to line associations. During the training step, ground-truth line segments are transferred to AFM and the network directly learns a model to minimize the similarity between the output and the true AFM. For testing, AFM representations are first generated from images, then a squeeze module [25] is used to iteratively group v(p) = p + a(p) belonging to the same line to fit line segments. Fig. 3(b) shows the detected line segments of the example image. It shows consistency with geometric 3D structures and robustness to texture noises.

B. 2D-3D line matching
For a single frame, the main steps to obtain 2D-3D correspondences consist of initial camera pose prediction, visible 3D lines collection, and individual 2D-3D line correspondence estimation. Here the extraction of 3D lines in FoV helps improve the efficiency because local 3D lines in FoV are very limited compared with all 3D lines in the 3D map. Considering the occlusion checking is difficult to conduct on only 3D lines map, we keep all the 3D lines in FoV without discarding occluded lines.
For an image at time t, the corresponding pose estimation from VINS-Mono is denoted asP t ∈ SE(3), and the estimated pose from 2D-3D correspondences is denoted as P t ∈ SE(3). By using the estimated pose from 2D-3D correspondences of the last frame P t−1 and the camera motion from VINS-Mono T, the predicted pose of current frameP t can be computed With the pose predictionP t , the local 3D lines {L t } in FoV can be extracted based on the two endpoint projections for efficiency. For checking the visibility of a 3D point P in FoV, where K is the camera intrinsic matrix. If w > 0, 0 ≤ x/w < column and 0 ≤ y/w < row, the 3D point is in FoV. For a 3D line segment, the visibility checking is more complicated. Based on the visibility of two endpoints, there are three cases for validation:  Fig. 4(a), we keep the whole 3D line segment as a local visible feature. • When only one endpoint is in FoV (in Fig. 4(b)), we iteratively sample new 3D points on the 3D line segment from the visible point by 0.1 ratio of the line length and check the visibility of the new sample points. The generated subset 3D line segment with the longest length in FoV is stored as a local visible feature. • When both two endpoints are out of FoV but a subsegment is in FoV, as shown in Fig. 4(c), we can also sample points to extract a subsegment in FoV. However, most of the invisible 3D lines are in this case, thus we discard all the 3D segments with two endpoints out of FoV for efficiency. Now with the predicted camera poseP t , the 2D lines {l t = (p s t , p e t )} are directly matched with the local 3D lines in FoV, where p s and p e are the start point and endpoint of a 2D line, respectively. For each possible 2D-3D correspondence, we use a 3D vector to measure the similarity, 2D angle distance θ, the distance d of two 3D endpoint projections to the corresponding infinite 2D lines, and the overlapped length len overlap of the finite 2D lines with the 3D line projection.
The projection of a 3D line L = (P s , P e ) on the image plane is l u = (p s u , p e u ) (Eq. 3). The normalized orientation of 2D lines is denoted as v = (p e − p s )/||p e − p s ||. Then the 2D angle distance θ can be computed by Assume the parametric representation of the extracted 2D line features is Ax + By + C = 0. The distance d can be computed by By using the point-to-finite-line projection points, we can compute the overlapped length len overlap α * = arg min where α s * and α s * are corresponding to the projection points of p s u and p e u respectively. Then for each extracted 2D line l i t , brute-force searching strategy is used to find a 3D line whose distance θ < θ 0 and d < d 0 . Thus we obtain a set of coarse 2D-3D line correspondences for further camera pose estimation.

C. Pose optimization and correspondence refinement
For a single frame, we can optimize the camera pose by minimizing the point-to-infinite-line distance of the two 3D endpoint projections to corresponding 2D line distance. The Lie algebra of the estimated camera poses P t are denoted as ξ t . The coefficient vector of the infinite 2D line as H = A B C . Considering all the 2D-3D line correspondences, the minimization function is where L i contains two endpoints, M denotes the number of correspondences. It is formulated as a non-linear leastsquares minimization problem. We initialize the camera pose with the predicted poseP t from VINS-Mono. With Lie algebra, the typical L-M algorithm can find the optimal camera pose P t . However, single frame 2D-3D correspondence observations are not robust enough for online camera localization. When the 3D lines in FoV are limited or parallel to each other in 3D space, the 2D-3D correspondences cannot constrain the 6-DoF pose. Additionally, even the correspondences are enough for pose estimation, the geometric localization noises of both 2D and 3D lines will make the estimation jitter around the true pose. To solve these problems, a sliding window is utilized to add more previous correspondence observations for optimizing the current pose P t (as shown in Fig. 5). Assuming the camera motion estimated from VINS-Mono for two adjacent keyframes is   Fig. 5: Pose estimation in the sliding window accurate, which is reasonable because the drifts of VINS-Mono on two adjacent keyframes are small. T 1 t , T 2 t , · · · , T n t are denoted as the individual camera motion of two adjacent keyframes from frame t to frame t − n. The camera pose P t−n of the previous n-th keyframe can be formulated as Then all previous 2D-3D correspondences in the sliding window can be "visible" in current frame. In Eq. 9, the previous camera pose P t−n is related to the current pose P t , while (T 1 t T 2 t · · · T n t ) −1 is a constant pose transformation, its Lie algebra is denoted as ∆ξ n t . Thus the pose optimization function can be written as where N is the number of previous frames in the sliding window. Therefore, more observations make the estimation more robust to outliers, and the motion constraints smooth the pose trajectory. When using the sliding window, an unbalance problem will arise due to the different numbers of 2D-3D correspondences for each frame. To equalize the contribution of each frame correspondences and improve the efficiency, a threshold for the maximum number of correspondences is set to discard the 2D-3D correspondences with short overlap distance len overlap . We employ Ceres Solver [27] to implement the optimization. After an optimized camera pose is obtained, the 2D-3D correspondences can be reestimated by the updated pose to reject outliers. It follows the 2D-3D line matching in Section. III-B with more restrict thresholds (e.g., θ 0 = 0.8 * θ, d 0 = 0.8 * d 0 ). Then a more accurate camera pose can be updated with the new correspondences. After several iterations, both camera pose and 2D-3D correspondences can be optimized.

IV. EXPERIMENTAL RESULTS
We tested the proposed method on two different realworld experiments. The first experiment was conducted on the public available EuRoC MAV Dataset [28] with groundtruth trajectories. Secondly, we performed real experiments on our dataset collected by a Realsense D435i camera under varying conditions to validate the performance of the proposed method.

A. Implementation
Considering the 3D LiDAR maps are often large scale with a big data volume, we first sub-sample the point clouds using CloudCompare [29] with a constant space resolution (1-5cm in experiments). Then the 3D line extraction based on [24] is conducted to obtain 3D lines. Since the arbitrary pose initialization in maps is a kidnapped robot problem for global 2D-3D correspondence estimation, a coarse pose initialization is given for the first frame by the PnP solver [23] on manually labeled 2D-3D point correspondences (at least 4 pairs, we use 6 pairs for robustness). For the learningbased 2D line extraction, we directly use the trained U-Net model [26] from Wireframe dataset [30] and modify it for online line segment detection. The testing platform is a desktop with Intel Core i7-4790K CPU, 32GB RAM, and an Nvidia GeForce GTX 980Ti GPU. The GPU is only used for 2D line detection.
During the 2D-3D correspondence estimation, the angle distance threshold θ 0 is set as 10 degrees to constrain the 3D projections to be almost parallel with corresponding 2D lines. Then the point to line distance threshold d 0 is set around 20 ∼ 30 pixels to collect matching pairs. If the number of correspondences for an image is less than a threshold (set as 8 empirically), it is identified unstable therefore the camera pose is predicted by camera motion only. If the number exceeds a threshold, we discard the correspondences with short len overlap distance for efficiency (empirically M 0 = 40, window size N = 10, there will be at most 440 correspondences in the sliding window). In the experiments, 2 or 3 iterations of optimization are often enough to obtain stable camera pose and 2D-3D correspondences.
Since our method is based on monocular visual-inertial odometry without loop closure, we compare it with the two versions of VINS-Mono [2], i.e., with or without loop closure. Loop closure helps for reducing the overall absolute trajectory error and mapping, but the refinement of the past poses in the loop does not help for realtime localization. Additionally, it causes pose shifts for current pose which have side effects for robot navigation.

B. Result on EuRoC MAV Dataset
The EuRoC MAV Dataset [28] is a visual-inertial datasets collected onboard a UAV. The datasets contain stereo images (20Hz), synchronized IMU data (200Hz), accurate groundtruth trajectories about 70 meters and a scan LiDAR map. The subset room data consist of 2 LiDAR maps wherein 3 video sequences each. 2D-3D correspondence results and estimated trajectory in LiDAR maps are shown in Fig. 6. All the 3D lines in FoV are projected to the image plane without occlusion checking. From the top left image, we can observe that the projections of 3D lines (in green) are shifted by using the estimated pose of VINS-Mono(odom). Then by iteratively optimizing 2D-3D correspondences and camera poses, stable 2D-3D correspondences are obtained with a more accurate pose. The position drifts are greatly reduced and stable 2D-3D correspondences can be estimated.
For quantitative analysis, former 200 estimated states are used for trajectory alignment with ground-truth and it is consistent for all methods [32]. The absolute trajectory error (ATE) [31] results are shown in Table I, it is clear that the 2D-3D correspondences improve the pose estimation accuracy compared with only odometry. The V2 room has more noises making the 2D-3D correspondences sometimes unstable for pose optimization. This is the reason why the  improvements for the V2 room are less significant than the results of the V1 room. The worst case is that no stable 2D-3D correspondence is available and the final estimations follow the odometry. Furthermore, our method shows competitive results compared with VINS-Mono(loop). While the loop closure optimizes the past poses in the loop and produces a pose jump for the current pose. For the realtime localization purpose, the refinements of the poses in the past make no sense and the pose jumps have side effects for navigation. Our method always estimates the current poses in the sliding window, which greatly reduces the drifts and does not have the issue of pose jumps. The relative pose errors (RPE) [32] on V 1 02 medium data are shown in Table II and Fig. 7, which are used to show the growth of position error with the trajectory length. The accumulated drift of VINS-mono(odom) is growing up with travel length. While the error of our method keeps small and stable along the way, which is related to the accuracy of 2D and 3D line localization.  To further evaluate our method under varying urban environments, we tested it on our own collected data of indoor corridors and outdoor buildings. An Intel RealSense D435i camera is used to collect synchronized images and IMU data. The left global-shutter imager sensor captures monocular image sequences (640 × 480 pixels images at 30Hz, the IR projector turned off) with synchronized IMU data (200Hz). The LiDAR maps are obtained by registering several scans of a FARO laser scanner focus3D S, as shown in Fig. 8. Both the indoor corridors (Fig. 8a) and Smith Hall (Fig. 8c) have a lot of occlusions, while the NSH building (Fig. 8b) is much simple with fewer occlusions. For these experiments, the trajectories are in the same pattern to run a complete round and return to the start points to see the position drifts. The loop closure is not stably detected for all runs, so the results of VINS-Mono with loop closure are not discussed here. The indoor corridor's results are shown in Fig. 1. The results of two outdoor buildings are shown in Fig. 9. Considering we do not have ground-truth trajectories, the estimation accuracy validation is shown in the following two ways. For the qualitative analysis, the 3D line features are projected to the image plane using the estimated poses of VINS-Mono(odom) and our method. For VINS-Mono odometry, it can be observed that the 3D line projections (green lines) are shifted and scaled by inaccurate camera poses in the top left images. While using our method, the pose estimations provide more accurate and stable 2D-3D structure correspondences in the bottom left images. Additionally, we can observe obvious misalignment between the two trajectories. More online correspondence and localization results are shown in the video 1 .  For the localization accuracy evaluation, we pick 5 frames along the trajectories and manually label 10 pairs of 2D-3D point correspondences, then use the PnP solver to estimate the ground-truth camera pose of these frames. The 5 frames are sampled along the trajectories on different times (t = β * t 0 , t 0 is the total running time) and the RMSE positions along 5 runs are used as the final ground-truths. The position errors are shown in Table III. The accumulated drifts increase a lot along the trajectories for VINS-Mono(odom). While our method greatly improves the localization accuracy with the assistance of stable 2D-3D line correspondences. The localization error keeps very small along the long trajectories. Another interesting observation is that the accumulated error can drift back if we keep the direction of the system and backward to the start position, which is shown in the results of VINS-Mono on NSH building. In terms of efficiency, the VINS-Mono does not use map information and can be customized set for output frequency. However, with different setting frequencies, the odometry results change a lot. We select the most stable one at 15 Hz. Then for the estimation of 3D correspondences and the camera pose, it costs about 0.012 seconds for each keyframe. Since the 3D line extraction is offline before the system starts, 2D feature extraction can run on 640 × 480 images at 25Hz, our method can be finally running at 15 Hz.

V. CONCLUSION
In this paper, we presented a novel monocular camera localization approach in prior LiDAR maps of urban environments. With the 3D geometric lines from LiDAR maps and robust online 2D line detection, our method efficiently obtains coarse 2D-3D line correspondences based on the camera motion prediction from VINS-Mono. The pose optimization with 2D-3D correspondences greatly reduces the pose estimation drifts of VIO system without using visualrevisiting loop closure. Both qualitative and quantitative results on real-world datasets demonstrate that our method can efficiently obtain reliable 2D-3D correspondences and accurate camera pose in LiDAR maps. As future work we intend to enhance the robustness of 2D-3D correspondences on inaccurate pose predictions, such as directly using the pose of the last frame as the prediction.