Point-line Visual-Inertial Odometry Systems under Short and Long Line Segment Fusion

Abstract

In visual-inertial odometry (VIO) research, combining point and line features can satisfy high requirements of integrity and restoration for scene reconstruction. In this paper, an integrating strategy of two types of line segments is proposed on the basis of utilizing point and line features for VIO frameworks. First, line segments are classified as short and long categories after extraction and tracking to enhance feature utilization. Then, short lines are spliced and long lines are merged on the basis of distance constraint and consistent direction. In the meanwhile, considering epipolar constraints during back-end processing, triangulation of the two line categories is optimized on the premise of correct matching between adjacent frames, so as to improve accuracy of pose estimation. Finally, point and line feature constraints are utilized simultaneously to reconstruct environments with back-end optimization. Evaluation on the EuRoC dataset shows that localization accuracy and robustness are improved with the proposed strategy.¹

Keywords

visual-inertial odometry point and line features line segment integration triangulation

Introduction

Simultaneous localization and mapping (SLAM) techniques include estimation of robot-sensor states and construction of environment models. In past decades, SLAM has developed rapidly in plenty of robotic fields, and it mainly includes laser SLAM (Badalkhani et al., 2021; Li et al., 2023), visual SLAM (Li et al., 2024), and other methods regarding various sensors (Huang, 2019; Wang et al., 2023). In various types of visual SLAM, monocular vision SLAM has been developed with the most frequency (Li et al., 2016), but the camera is prone to take a variety of blurry photos leading to difficult positioning. To make up for camera shortcomings, camera measurements are usually fused with inertial measurement unit (IMU) ones in practical applications, where an IMU can obtain motion information inside the robot at high frequency without being affected by surrounding environments. On the other hand, a camera completes loopback detection and correction through visual matching, where cumulative IMU drift is corrected effectively. One classic system combining monocular camera and IMU is VINS-Mono (Qin et al., 2018), which integrates point feature extraction and matching, IMU extraction and pre-integration (Forster et al., 2016), system initialization, sliding window optimization (Bartoli et al., 2005), and loop detection, providing a complete visual-inertial odometry framework.

The main element adopted by most map reconstruction methods is point feature, which is large amount, less pixel occupancy, and low computational complexity. However, point features are sensitive to brightness so that tracking process fails under unstable light condition, and they neglect scene structure and are also difficult to be detected in low-texture environments (Leutenegger et al., 2014). On the other hand, the appearance of line features makes up for the shortcomings of point ones. The PL-SLAM method in Gomez-Ojeda et al. (2018) uses BoW technologies to collect descriptors of line segments into total words in the closed-loop detection phase, which improves robustness but decreases real-time performance. He et al. (2018) propose a method by combining point and line features under VIO frameworks, where two parametric representation patterns are introduced for line features and an observation model is defined to integrate line features. The method in Fu et al. (2020) optimizes the line segment detection process on the basis of He et al. (2018), and makes hidden parameters explicit and speeds up feature detection. Lee et al. (2021) design the PLF-VINS method by exploring position correlation of points and line features, and by coupling residual of point and line features with a designed constraint.

In this paper, a point-line visual odometer method is proposed by designing short line splicing and long line fusion strategies for pose estimation and scene reconstruction, which improves feature processing performance and scene restoration degree in the loop detection phase. Firstly, two judgment constraints are designed to filter and classify line features, and integration for line segments is performed on the premise of consistent direction and close distance in adjacent two keyframes. Then, original twice triangulation operations are reduced to once by merging endpoints of two short lines, and triangulation for parallel long line features is made based on correct line segment selection and matching. Consequently, short line splicing enhances triangulation accuracy and long line fusion makes correct matching, and then VIO accuracy can be improved. Then, the reprojection error model of line features is analyzed by deducing Jacobin matrices on normalized planes. Finally, reprojection errors of lines are combined with those of points into sliding windows to achieve state optimization. Performance of the proposed method is verified on the EuRoC micro aerial vehicle dataset. Main contribution of the paper is that line segment utilization is enhanced by designing splicing and fusion strategies, and feature matching accuracy is enhanced and feature processing time is saved in the triangulation process.

Related Work

Back-end Operation of Point-Line Based VIO

Two patterns of back-end processes are filter-based and optimization-based for visual SLAM systems. One classic method of the filter-based pattern is MSCKF (Mourikis et al., 2007), which predicts localization for a new frame through IMU integration and then takes feature reprojection as observation. Zheng et al. (2018) propose a binocular visual-inertial odometer by using a Kalman filter to fuse point and line features and by designing geometric constraints based on normal vectors of lines. Kong et al. (2015) combine points and lines based on trifocal geometry among image triplets, with filtering algorithms to correct pose information in real time. PL-VIO (He et al., 2018) and PL-VINS (Fu et al., 2020) methods are based on the optimization pattern to obtain pose information with point-line feature fusion. Li et al. (2022) utilize tight coupling to combine carrier phase data obtained from GPS measurements with visual inertia to obtain a highly accurate and low-noise P³-VINS system.

Vanishing Points for VIO

Vanishing points are introduced for line feature classification in VIO, which improves localization accuracy but decreases real-time performance. Zou et al. (2019) propose the Manhattan hypothesis, where line features are induced into structural lines in three directions, and points link with lines through vanishing points. Then, Xu et al. (2022) apply the Manhattan hypothesis to VIO frameworks, and define lines that are failed to track as unstructured lines. Without the Manhattan assumption, the method proposed by Lim et al. (2022) directly uses vanishing points to perform structure mapping, and adds a new estimator to back-end optimization according to the relationship between lines and vanishing points. The line feature degradation problem is discussed in Lim et al. (2021), which exists in the triangulation process and is caused by pure translation in point-line visual SLAM algorithms. To extract vanishing points, Lu et al. (2017) propose a polar-grid building method, Camposeco et al. (2015) propose a one-line method to classify line segments, and Bazin et al. (2012) propose a three-line method to solve the minimum sampling problem. Feng et al. (2024) propose an image processing algorithm with an adaptive strategy by grouping and merging line segments, which reduces difficulty of matching line features and improves localization accuracy in low-light environments.

Line Segment Detection

For line feature processing, gradient sorting is performed first by calculating gradient magnitude in the image, and then a region growth algorithm is used to detect line features. Classic line segment detection and matching methods are line segment detector (LSD) (Von Gioi et al., 2010) and line band descriptor (LBD) (Zhang et al., 2013), respectively. In Fu et al. (2020), scale and layer number of the Gaussian pyramid are simplified to speed up the detection process, and a length optimization strategy is proposed to filter short lines. The method proposed in Wen et al. (2020) adds line information to a stereo vision odometer on the basis of VINS-Fusion (Qin et al., 2019). The method proposed in Liu et al. (2021) uses the EDLine method (Akinlar et al., 2011) for taking less time on line extraction than LSD, and designs a line merging algorithm for short lines. Lim et al. (2021) employ the ELSED method (Suarez et al., 2022) to accelerate line feature detection by reducing candidate pixels in pixel chains (Lim et al., 2022). Zhang et al. (2024) propose a vanishing point estimation method with line segment consistency using structural constraints, where line features are classified into structured and unstructured lines on the basis of vanishing points.

Line Feature Integration

System Overview

In this paper, optical flow tracking, point and line feature extraction, and IMU pre-integration are performed in the data preprocessing phase. By filtering and classifying line segments, strategies are designed in the front-end for short line splicing and long line fusion. After feature triangulation, it enters back-end optimization to recover scene information, where point and line reprojection error, IMU motion constraint, and marginalization are jointly optimized in sliding windows.

Point and Line Extraction

For point feature processing in the proposed method, extracted points are Shi-Tomasi corners (Shi et al., 1994), and the Kanade-Lucas-Tomasi (KLT) (Tomasi et al., 1991) algorithm is used to track feature points between adjacent frames. The F-matrix test is preformed and the random sample consensus (RANSAC) scheme (Hartley et al., 2003) is used to eliminate abnormal points.

For line feature processing, the LSD algorithm is employed in the method to detect, recognize, and track line segments without parameter tuning. After line feature tracking, LBD and k-nearest neighbor (KNN) algorithms are adopted to describe and match line features between two adjacent frames.

Regarding line feature detection, parameters of the image pyramid are adjusted to generate an LSD detector, and the detected line segments are gathered together to further calculate line feature descriptors (Fu et al., 2020). Lines are marked by fixed and dynamic flags, and then matched with a binary descriptor in previous and current frames (Liu et al., 2021). Thresholds for descriptor distance are set, and matching is successful if moving distance of line endpoints on adjacent frames is within the thresholds.

Short Line Splicing

A line segment splicing strategy is designed first to tackle the scattering problem caused by line feature detection. In the process of LSD execution, a complete line segment in the space is often divided into several irregular ones, which breaks integrity of line features and increases time costs of back-end triangulation. Symbols $l_{1} ″ (s_{1} ″, e_{1} ″)$ and $l_{2} ″ (s_{2} ″, e_{2} ″)$ denote two lines detected by LSD, with $s_{1}, e_{1}, s_{2},$ and $e_{2}$ as endpoints.

This line representation manner distinguishes start and end points of a line segment, which makes a spatial line segment have direction. Initially, a constraint condition of line direction judgment on each frame is performed after LSD detection:

{\begin{matrix} l_{2} (e_{2}, s_{2}) = l_{2} ″ (s_{2} ″, e_{2} ″), if (x_{s_{2}} - x_{e_{2}}) / (x_{s_{1}} - x_{e_{1}}) < 0, \\ l_{2} = (s_{2}, e_{2}), if (x_{s_{2}} - x_{e_{2}}) / (x_{s_{1}} - x_{e_{1}}) \geq 0 \end{matrix}

(1)

where $x_{(\cdot)}$ represents abscissa of an endpoint. When $l_{1}$ and $l_{2}$ are in opposite directions, coordinates of start and end points of the later detected $l_{2}$ are exchanged to keep the same direction with the early detected $l_{1}$ .

Two screening conditions are then set with respect to distance and angle, respectively, to judge which lines should be tackled and to weed out interfering lines. In the first one, the two closest endpoints are selected among the four endpoints of $l_{1}$ and $l_{2}$ , and the distance of two candidate segments is less than the set threshold $d_{t}$ . In the second one, to ensure that the spliced line segments are originally from a long line, the angle $θ_{t}$ between the two short lines must be $0$ rad or $π$ rad under tolerance of 5 degrees. The conditions regarding short lines are illustrated in Figure 1(a).

At last, short lines $l_{1}$ and $l_{2}$ are spliced into a new complete line segment $l_{s}$ after executing the above three judgment conditions. A spliced line segment is with four situations of $(e_{1}, s_{2})$ , $(e_{1}, e_{2})$ , $(s_{1}, s_{2})$ , and $(s_{1}, e_{2})$ , as shown in Figure 1(b). As a consequence, the splicing operation simplifies direction types of candidate lines and forms suitable lines for tracking and matching.

Long Line Fusion

The LSD algorithm often reproduces a line to several repeated lines in scenes with regular spatial structure, such as window, sidewalk, and pipeline. These repeated lines are close to each other but not overlapped, which affects line feature matching and back-end feature recovery, and largely affects pose estimation of line features. To make lines concise in reconstructed scenes, two parallel lines are merged by the following conditions.

The fusion sketch for long line features is shown as (c) and (d) in Figure 1. After line segments are detected, two long lines defined as $l_{3} (s_{3}, e_{3})$ and $l_{4} (s_{4}, e_{4})$ are screened by two same-line criteria, where $l_{3}$ and $l_{4}$ are parallel to each other, and two pairs of corresponding endpoints for $l_{3}$ and $l_{4}$ are both under distance threshold $d_{t}$ .

Considering directivity of lines, there are four situations of two-line combinations as $(s_{3}, s_{4})$ , $(s_{3}, e_{4})$ , $(e_{3}, s_{4})$ , and $(e_{3}, e_{4})$ . Directions of candidate lines are adjusted according to (1) to make directions of $l_{3}$ and $l_{4}$ the same. Since $l_{4}$ is detected as a misplaced line on the basis of $l_{3}$ , the second line $l_{4}$ is merged by $l_{3}$ . As a consequence, two long lines are fused by judgments of (1) and the above two constraints.

Figure 2 shows the comparison results of extracted line features between the proposed method and the LSD algorithm. The line segments in the blue boxes in (a) are detected by the LSD algorithm, and they are processed by the proposed short line splicing strategy as shown in (b). Line segments in the blue boxes in (c) and (d) correspond to before and after utilizing the proposed long line fusion strategy, respectively.

Optimization For Line Features

Geometric of 3D Lines

With three degrees of freedom (DoFs) for a single three-dimensional (3D) point $[x, y, z]^{T}$ , a line by two endpoints has six DoFs theoretically. A straight line has four DoFs after abandoning DoFs of translation along its direction and rotation around itself. Typical geometric manners for representing lines include the two-parameter representation in StructVIO (Zou et al., 2019) and the multi-parameter representation in PL-VIO (He et al., 2018).

Six-parameter Plücker coordinates have advantages in operation and representation of a line, which is determined by difference and cross of vectors corresponding to two endpoints of a line. It is used in the paper jointly with four parameter orthogonal coordinates to represent a 3D line.

Under the Plücker coordinate system, a 3D line is represented as $L = [n^{T}, v^{T}]^{T}$ , where $n \in R^{3}$ is a normal vector and $v \in R^{3}$ is a direction vector both in a plane $π$ . The two vectors have the following relationship:

n^{T} v = 0 .

(2)

In the process of line feature detection from the $i$ th frame to the $j$ th frame, lines $L^{j}$ and $L^{i}$ meet the following transformation relationship:

H_{ji} = [\begin{matrix} R_{ji} & [p_{ji}] \times R_{ji} \\ 0 & R_{ji} \end{matrix}]

(3)

L^{j} = [\begin{matrix} n^{j} \\ v^{j} \end{matrix}] = H_{ji} L^{i}

(4)

where $H_{ji}$ is a transformation matrix from the $i$ th frame to the $j$ th frame, $R_{ji}$ is a rotation matrix converted between the two frames, and $[\cdot]_{\times}$ denotes the skew symmetric matrix of a 3D vector.

The constraint (2) in the six-parameter line segment representation implies over-parameterization for back-end optimization, which increases calculation complexity and system instability. Thus, the orthogonal coordinate with minimum standard is adopted when optimizing line features in the back-end. Linear transformation exists between the two representation manners, which can thus be jointly applied. According to QR decomposition (Bartoli et al., 2005), the Plücker coordinates of a line segment are transformed to the orthogonal coordinates $(U, W) \in$ $SO (3) \times SO (2)$ . Symbol $U$ denotes a 3D rotation matrix between line coordinate system and camera coordinate system, $W$ denotes a 2D rotation matrix, and they are calculated as follows, where $φ$ is a rotation angle:

\begin{matrix} U = [\begin{matrix} \frac{n}{| | n | |} & \frac{v}{| | v | |} & \frac{n \times v}{| | n \times v | |} \end{matrix}], \end{matrix}

(5)

W = [\begin{matrix} \cos φ & - \sin φ \\ \sin φ & \cos φ \end{matrix}] = [\begin{matrix} w_{1} & - w_{2} \\ w_{2} & w_{1} \end{matrix}] .

(6)

Based on (4) and (5), the Plücker coordinate can be expressed as follows:

[\begin{matrix} n & v \end{matrix}] = U [\begin{matrix} w_{1} & 0 \\ 0 & w_{2} \\ 0 & 0 \end{matrix}] = [\frac{n}{| | n | |} \frac{v}{| | v | |} \frac{n \times v}{| | n \times v | |}] [\begin{matrix} | | n | | & 0 \\ 0 & | | v | | \\ 0 & 0 \end{matrix}],

(7)

W = \frac{1}{\sqrt{| | n | |^{2} + | | v | |^{2}}} [\begin{matrix} | | n | | & - | | v | | \\ | | v | | & | | n | | \end{matrix}] .

(8)

The distance $d_{O}$ is from the Plücker coordinate origin to the 3D line, which satisfies $d_{O} = | | n | | / | | v | | = w_{1} / w_{2}$ , and thus $W$ contains information of $d_{O}$ . Thus, one DoF from $d_{O}$ and three DoFs from the rotation matrix constitute the orthogonal coordinate representation with four parameters. According the following formula, a space line $L$ represented by the orthogonal coordinate is transformed into $L'$ under the Plücker coordinate representation:

L' = {[\begin{matrix} \cos φ \cdot u_{1}^{T} & \sin φ \cdot u_{2}^{T} \end{matrix}]}^{T} = \frac{1}{\sqrt{| | n | |^{2} + | | v | |^{2}}} L

(9)

where $u_{i}$ is the $i$ th column of $U$ .

Triangulation of Integrated Line Features

In the triangulation process, line feature poses are estimated by the epipolar constraint. Mismatching in line feature triangulation increases depth calculation errors and causes position drift of recovered line features. Mismatching further affects a series of modules, such as transformation from image coordinates to camera coordinates, camera motion estimation, and reprojection errors.

These errors produce a cumulative effect in the whole triangulation process, which leads to inaccurate estimation for 3D line positions. In order to improve accuracy and robustness of the triangulation process, two cases of feature mismatching are analyzed and improved as follows.

Short line: The triangulation schematic with short segment merging is shown in Figure 3. Motion of line features between previous and current frames is represented by $R$ and $t$ , camera centers of two keyframes are $c_{0}$ and $c_{1}$ , image planes of the two cameras are $Π_{0}$ and $Π_{1}$ , and the connecting line of $c_{0}$ and $c_{1}$ produces two epipoles for triangulation.

Projection of a short line segment $l_{1} (s_{1}, e_{1})$ on two keyframes is denoted as $l'_{1} (s'_{1}, e'_{1})$ and $l_{1} ″ (s_{1} ″, e_{1} ″)$ , which can be detected by the LSD method, and projection of a short line segment $l_{2} (s_{2}, e_{2})$ on two keyframes is denoted as $l'_{2} (s'_{2}, e'_{2})$ and $l_{2} ″ (s_{2} ″, e_{2} ″)$ . It is noted that $l_{1}$ and $l_{2}$ are both recovered in existing methods, as shown by red and blue space lines in Figure 3.

After line matching between two adjacent frames, image lines are filtrated by short line splicing conditions in Section III.C. For selected image lines $l_{1} ″ (s_{1} ″, e_{1} ″)$ and $l_{2} ″ (s_{2} ″, e_{2} ″)$ on $Π_{1}$ , they are treated as $(s_{1} ″, e_{2} ″)$ by the short line splicing strategy in Section III.C. Similarly, the image line $(s'_{1}, e'_{1})$ is obtained on $Π_{0}$ according to the short line splicing strategy. Then, triangulation operation is conducted for $(s_{1} ″, e_{2} ″)$ and $(s'_{1}, e'_{1})$ , and the space line feature $l (s_{1}, e_{2})$ is recovered. Therefore, one time of triangulation is reduced, and triangulation accuracy for short lines can be enhanced by the integrated manner.

Moreover, a set is arranged regarding $Π_{1}$ to count the image line number of triangulation, where spliced line segments are included such as $(s_{1} ″, e_{2} ″)$ , and the following long lines as well as normal lines are also included. When triangulation lines do not reach a preset number, unmatched short lines are treated as supplements that are approximately vertical or horizontal, so as to enrich environment information and make recovered lines orderly.

Long line: For object edge and rich-texture region in images, there usually exist long line segments under the relationships of parallel and close. Accordingly, repeat line segments are detected on adjacent images $Π_{0}$ and $Π_{1}$ , and mismatching easily occurs for these lines. On the basis of screening constraints in Section III.D, these repeat lines are detected.

The triangulation process with long line fusion is shown in Figure 4. A long line $l_{3} (s_{3}, e_{3})$ projects on $Π_{0}$ and $Π_{1}$ , and $l_{4}' (s_{4}', e_{4}')$ on $Π_{0}$ and $Π_{1}$ , respectively. According to existing methods, the two spatial lines in Figure 4 are recovered lines. However, for line feature matching in existing methods, $l_{3}'$ easily mismatches with $l_{4} ″$ due to close distance, and similarly for $l_{3} ″$ with $l_{4}'$ .

For these line segments, they are tackled by the long line fusion strategy in Section III.D, such that $l_{4}'$ is recognized as $l_{3}'$ while $l_{4} ″$ is recognized as $l_{3}$ . Then, the triangulation operation is conducted for $l_{3}'$ and $l_{3} ″$ , and thus the spatial line $l_{3} (s_{3}, e_{3})$ is reconstructed. With this manner, long lines are matched more correctly, and unique spatial lines are recovered. Moreover, similar to the previous short line part, unmatched long lines that are approximately vertical or horizontal are treated as a supplement for the counting set.

Measurement Model for Line Feature Reprojection

Given a 3D line $L$ that is represented by the Plücker coordinate $P_{l}$ and orthogonal coordinate $O_{l}$ in Section IV.A, it can be transformed from the world coordinate system to the camera coordinate system by (4) to obtain $L^{c} = [{(n^{c})}^{T},$ $(v^{c})^{T}]^{T}$ . The line $L$ can be reprojected to the image plane as follows:

\begin{matrix} l^{c} = K n^{c} = [\begin{matrix} f_{y} & 0 & 0 \\ 0 & f_{x} & 0 \\ - f_{y} c_{x} & - f_{x} c_{y} & f_{x} f_{y} \end{matrix}] n^{c} \end{matrix}

(10)

where $K$ is the projection matrix of line features, $f_{x}$ and $f_{y}$ relate to camera focal length, and $c_{x}$ and $c_{y}$ relate to the camera imaging center. When a line projects to the normalized plane, $K$ is an identity matrix.

The residual vector $r_{l}$ of line reprojection relates to distance $d_{1}$ and $d_{2}$ from endpoints of the observed line $l'$ to the reprojected line $l^{c}$ , and $l'$ are parameterized as $l' = [L_{1}, L_{2}, L_{3}]^{T}$ . The residual vector can be updated as follows:

\begin{matrix} r_{l} = \frac{1}{| | z_{L}^{c} | |} [\begin{matrix} d_{1} (p_{s}, l^{c}) \\ d_{2} (p_{e}, l^{c}) \end{matrix}] \end{matrix}

(11)

where $z_{L}^{c}$ denotes the line segment on the normalized image plane projected by $L$ , and $p_{s} = [s_{x}, s_{y}, 1]^{T}$ and $p_{e} = [e_{x}, e_{y}, 1]^{T}$ are two endpoints of $l'$ . Moreover, distances in (11) are calculated as

d_{1} (p_{s}, l^{c}) = \frac{p_{s}^{T} l^{c}}{l_{a}}, d_{2} (p_{s}, l^{c}) = \frac{p_{e}^{T} l^{c}}{l_{a}}

(12)

where $l_{a} = \sqrt{L_{1}^{2} + L_{2}^{2}}$ .

Then, Jacobian matrices regarding line features are obtained by minimizing reprojection errors, which mainly include variation of IMU states $δ x_{I}$ and orthogonal coordinates $δ O_{l}$ . The corresponding Jacobian is as follows:

\begin{matrix} J_{l} = \frac{\partial r_{l}}{\partial l^{c}} \frac{\partial l^{c}}{\partial L^{c}} [\begin{matrix} \frac{\partial L^{c}}{\partial δ x_{I}} & \frac{\partial L^{c}}{\partial L^{w}} \frac{\partial L^{w}}{δ O_{l}} \end{matrix}] \end{matrix}

(13)

where $(\cdot)^{c}$ and $(\cdot)^{w}$ are symbols denoted under the camera coordinate system and the world coordinate system, respectively. Entities in (13) are shown as follows:

\begin{matrix} \frac{\partial r_{l}}{\partial l^{c}} = [\begin{matrix} \frac{- L_{1} (p_{s}^{T} l^{c})}{l_{a}^{3}} + \frac{s_{x}}{l_{a}} & \frac{- L_{2} (p_{s}^{T} l^{c})}{l_{a}^{3}} + \frac{s_{y}}{l_{a}} & \frac{1}{l_{a}} \\ \frac{- L_{1} (p_{e}^{T} l^{c})}{l_{a}^{3}} + \frac{e_{x}}{l_{a}} & \frac{- L_{2} (p_{s}^{T} l^{c})}{l_{a}^{3}} + \frac{e_{y}}{l_{a}} & \frac{1}{l_{a}} \end{matrix}], \end{matrix}

(14)

\frac{\partial l^{c}}{\partial L^{c}} = [K 0_{3 \times 3}] = [\begin{matrix} I_{3 \times 3} & 0_{3 \times 3} \end{matrix}],

(15)

\frac{\partial L^{c}}{\partial δ x_{I}} = T_{b}^{c} [\begin{matrix} {(R_{b}^{w})}^{T} {[v^{w}]}_{\times} \\ 0_{3 \times 3} \end{matrix} \begin{matrix} {[{(R_{b}^{w})}^{T} (n^{w} + {[v^{w}]}_{x} T_{b}^{w})]}_{\times} \\ {[{(R_{b}^{w})}^{T} v^{w}]}_{\times} \end{matrix} 0_{6 \times 9}],

(16)

\frac{\partial L^{c}}{\partial L^{w}} \frac{\partial L^{w}}{\partial δ O_{l}} = T_{b}^{c} [\begin{matrix} 0_{3 \times 1} & - w_{1} u_{3} & w_{1} u_{2} & - w_{2} u_{1} \\ w_{2} u_{3} & 0_{3 \times 1} & - w_{2} u_{1} & w_{1} u_{2} \end{matrix}]

(17)

where $T_{b}^{c}$ is the transformation matrix from the IMU coordinate system to the camera coordinate system, $R_{b}^{w}$ and $T_{b}^{w}$ are the rotation matrix and translation vector from the IMU coordinate system to the camera coordinate system, respectively, and $I_{3 \times 3}$ is an identity matrix.

System Optimization Based on Sliding Windows

With initial pose estimation and IMU state prediction, the proposed method is optimized by using the tightly coupled pattern based on sliding windows. In the process of bundle adjustment (BA) optimization, a fixed number of sliding windows (Shen et al., 2015) are used to dynamically delete and add state variables and prior information, so as to constrain scale of optimization variables and maintain real-time performance of the system.

The factor graph is based on sliding window optimization, and features include point and line. According to point and line fusion requirement, a line feature reprojection error term is designed and added for the minimum cost function, which is composed of cost term sum from all state variables as

\begin{matrix} min_{χ} {\sum_{i \in B} ρ_{b} (| | r_{b} (z_{b_{i} b_{i + 1}}, χ) | |_{\sum b_{i} b_{i + 1}}^{2}) + | | r_{0} - J_{0} χ | |^{2} \\ + \sum_{(i, j) \in P} ρ_{p} (| | r_{p} (z_{p_{j}}^{c_{i}}, χ) | |_{\sum_{p_{j}}^{c_{i}}}^{2}) + \sum_{(i, k) \in L} ρ_{l} (| | r_{l} (z_{l_{k}}^{c_{i}}, χ) | |_{\sum_{l_{k}}^{c_{i}}}^{2})} \end{matrix}

(18)

where $B$ , $P$ , and $L$ are sets of all IMU, point feature information, and line feature information in sliding windows.

In (18), $r_{b} (z_{b_{i} b_{i + 1}}, χ)$ is the IMU measurement residual between $i$ th and $i + 1$ th body coordinate systems, $r_{p} (z_{p_{j}}^{c_{i}}, χ)$ and $r_{l} (z_{l_{k}}^{c_{i}}, χ)$ are measurement residuals of points and lines that are observed at least twice in sliding windows, respectively. Symbols $z_{b_{i} b_{i + 1}}$ , $z_{p_{j}}^{c_{i}}$ , and $z_{l_{k}}^{c_{i}}$ correspond to measured values of IMU, point, and line, respectively. Symbol $r_{0}$ denotes marginalized prior information residual, and $J_{0}$ is a Jacobian matrix of prior information. Covariance matrices of IMU, point feature, and line feature are denoted by $\sum b_{i} b_{i + 1}$ , $\sum_{p_{j}}^{c_{i}}$ , and $\sum_{l_{k}}^{c_{i}}$ , respectively. Symbols $ρ_{b}$ , $ρ_{p}$ , and $ρ_{l}$ are loss functions in the optimization process for IMU, point feature, and line feature, and they are all set to the Huber norm function.

Experimental Validation

In the section, experiments on the EuRoC micro aerial vehicle dataset (Burri et al., 2016) and TUM-VI Benchmark dataset (Schubert et al., 2018) are conducted to test performance of the proposed method. The EuRoC dataset contains a synchronized global shutter stereo camera with 20 FPS and IMU measurement data with 200 Hz. The TUM dataset is recorded by a handheld fisheye stereo Kinect camera, and seven sequences are selected for the experiments. For evaluation, the proposed method is compared with VINS-Mono (Qin et al., 2018), PL-VINS (Fu et al., 2020), and UV-SLAM (Lim et al., 2022) under eight sequences without loop closure, and Figure 5 shows experiment snapshots on the eight sequences running by the proposed method.

Parameters of the compared methods are based on default values for comparison, and the threshold distance $d_{t}$ in the proposed method for line feature integration is set to 10 pixels by trial and error. The platform laptop is configured as Intel Core i7-11800H CPU @2.30 GHz with 16 GB RAM, and under Ubuntu 16.04LTS with ROS Kinetic. We accomplish the proposed strategy on the basis of the method PL-VINS (Fu et al., 2021), and we provide our detailed implementation on the website.¹

APE Comparison

As a common evaluation index in the visual SLAM system, absolute pose error (APE) directly reflects VIO accuracy and global consistency of a trajectory. The root mean square error (RMSE) is obtained by comparing localization on trajectory with truth value, so as to show effect of the designed line integration process. Table 1 shows RMSE results in translation of the three methods without loop closure, where data in bold are closest to benchmark trajectories. It can be found that the proposed method shows superior performance in five sequences.

Table I.

RMSE APE(meter) of VINS-MONO, PL-VIN, UV-SLAM, and the proposed method without loop closure regarding the EuRoC dataset.

Experiment	VINS-Mono	PL-VINS	UV-SLAM	Proposed
MH_01_easy	0.174	0.157	0.212	0.152
MH_02_easy	0.178	0.170	0.173	0.169
MH_03_medium	0.195	0.227	0.247	0.227
MH_04_difficult	0.348	0.304	0.356	0.297
MH_05_ difficult	0.302	0.281	0.325	0.276
V1_01_easy	0.089	0.069	0.078	0.069
V1_02_medium	0.111	0.123	0.106	0.122
V1_03_ difficult	0.188	0.181	0.183	0.178
Average	0.198	0.189	0.210	0.186
Enhance	6.45%	1.46%	12.9%	—

Compared with VINS-Mono, PL-VINS, and UV-SLAM methods, pose estimation accuracy of proposed algorithm is improved by 6.45%, 1.46%, and 12.9%, respectively. Figure 6 shows scene reconstruction during MH_04_difficult and V1_03_medium experiments by the proposed method, where the real environment is described with rich line features.

To demonstrate the results more intuitively, localization paths on the xy -plane are also provided by VINS-Mono, PL-VINS, and the proposed method on MH_04_difficult and V1_03_difficult sequences, as shown in Figure 7. MH_04_difficult is the most challenging sequence in EuRoC due to aggressive motion, feature-less area, and remarkable illumination change. It is seen from Figure 7 that paths estimated by the proposed method are closest to ground truth, especially in turning and accelerating areas. Meanwhile, localization results have been improved by the proposed method during the process.

Table 2 shows the RMSE values of VINS-Mono, UV-SLAM, and the proposed method regarding seven sequences of the TUM dataset, where data in bold are closest to benchmark trajectories. It is seen that the proposed method performs well in five sets of sequences compared with the two compared methods.

Table 2.

RMSE APE(meter) of VINS-MONO, UV-SLAM and the proposed method without loop closure regarding the TUM dataset.

Experiment	VINS-Mono	UV-SLAM	Proposed
room1_512_16	0.094	0.079	0.075
room2_512_16	0.069	0.068	0.063
room3_512_16	0.248	0.127	0.125
room4_512_16	0.041	0.042	0.049
room5_512_16	0.340	0.216	0.208
room6_512_16	0.071	0.069	0.073
slide1_512_16	0.583	0.495	0.457
Average	0.198	0.210	0.150
Enhance	32.0%	40.0%	—

Figure 8 and Figure 9 show the localization paths of VINS-Mono, UV-SLAM, and the proposed method for room1_512_16 and room5_512_16 sequences, respectively. It can be seen that the paths by the proposed method is closest to truth value.

The VINS-Mono system extracts point features, the PL-VINS system uses the LSD algorithm to extract line features, and the UV-SLAM system uses ELSED algorithm to detect line features. As a comparison, the proposed method corrects false detection phenomenon of LSD on the basis of PL-VINS. The line features in the actual scene are restored by utilizing the splicing and fusion strategies, which improves localization accuracy of the system. Due to the number and distribution of line features in different scenes, these datasets show different performance enhancements. For example, since spatial structures are rich in the MH_04_difficult and MH_05_difficult datasets, the trajectory errors are obviously smaller than those of the compared three methods.

RPE Comparison

The relative pose error (RPE) (Hartley et al., 2003) is usually utilized to evaluate visual odometer drift in a fixed time interval, and translation and rotation errors are selected to evaluate estimated trajectories. The time interval for evaluation is set to be the default value 0.01 s. Table 3 shows RPE evaluation results of VINS-Mono, PL-VINS, and the proposed method under eight sequences. By jointly optimizing point and line features, the proposed method provides best performance in translation and rotation errors with six and four sets of data, respectively, as shown in Table 3.

Table 3.

RMSE RPE evaluation of VINS-MONO, PL-VINS, and the proposed method regarding the EuRoC dataset.

Experiment	VINS-Mono		PL-VINS		Proposed
	Tran.	Rot.	Tran.	Rot.	Tran.	Rot.
MH_01_easy	1.251	0.950	1.244	0.968	1.244	0.967
MH_02_easy	0.027	0.017	0.028	0.017	0.028	0.015
MH_03_medium	0.042	0.021	0.036	0.039	0.035	0.039
MH_04_difficult	0.043	0.045	0.039	0.036	0.038	0.035
MH_05_difficult	0.054	0.018	0.051	0.020	0.050	0.019
V1_01_easy	0.089	0.057	0.082	0.059	0.081	0.059
V1_02_medium	0.073	0.078	0.040	0.072	0.039	0.067
V1_03_difficult	0.055	0.095	0.046	0.058	0.045	0.057

After comparing with PL-VINS, the proposed line processing algorithms play a key role in reducing cumulative drift. Since different sequences correspond to various environments, results in Table 2 show that the proposed line integration algorithms improve accuracy and stability of localization in both complex factories and simple indoors. Moreover, the proposed method reduces local drift of camera rotation and translation, which enhances reliability and robustness of the VIO system.

Time Cost Comparison

It is mentioned in Section IV.B that time in the triangulation process is shortened by integrating lines. To illustrate the effectiveness of the proposed method, time for designed line segment detection and matching algorithms is recorded and compared with PL-VINS, and results are shown in Table 4. The test is also carried out on the above eight sequences, and each final result is obtained through averaging five sets of experiments.

Table 4.

Line detection and matching time of PL-VINS and proposed methods regarding the EuRoC dataset.

Experiment	LSD+LBD+KNN	LSD(Proposed)+LBD+KNN	Reduced
MH_01_easy	16.33	15.98	2.15%
MH_02_easy	16.09	13.45	19.63%
MH_03_medium	14.65	14.22	3.03%
MH_04_difficult	12.58	12.47	0.88%
MH_05_ difficult	13.43	12.60	6.59%
V1_01_easy	12.54	12.47	0.56%
V1_02_medium	11.15	11.00	1.36%
V1_03_ difficult	10.14	9.98	1.60%

For VIO systems of point and line combination, it is noted that line feature processing costs much time, and real-time performance is enhanced with decreased line feature processing time. It is seen from Table 4 that the proposed method reduces time cost in different degrees for both laboratory and factory environments, especially the time cost of LSD+LBD+KNN being reduced by 19.63% for MH_02_easy. Thus, the proposed method improves efficiency of VIO systems by enhancing line feature processing.

Conclusion

A novel visual-inertial odometry method is proposed in the paper based on point-line feature fusion and line feature integration processing. In feature processing of the front-end stage, algorithms for line segment splicing and fusion are designed, which improves utilization of valid line segments and makes feature information more abundant in scene reconstruction. In optimization of the back-end stage, triangulation processes of two types of line features are designed, which reduce triangulation time of line features and improve matching rate of line features without loss of localization accuracy. Finally, IMU error, updated line feature residual, point feature reprojection error, and prior information are fused into sliding windows, and the cost function of each part is provided for optimizing system states. Through comparative experiments, it is found that the proposed method has good accuracy especially in challenging scenes.

Footnotes

Declaration of conflicting interests

The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported in part by the National Natural Science Foundation of China under grant 62473282, 62303349, 62203326, and 61973234.

ORCID iDs

Xiaojing He

Baoquan Li

Notes

References

Akinlar

Topal

(2011) EDLines: A real-time line segment detector with a false detection control. Pattern Recognition Letters 32(13): 1633−1642.

Badalkhani

Havangi

Farshad

(2021) An improved simultaneous localization and mapping for dynamic environments. International Journal of Robotics and Automation 36(6): 374−382, 2021.

Bartoli

Sturm

(2016) Structure-from-motion using lines: Representation, triangulation, and bundle adjustment. Computer Vision and Image Understanding 100(3): 416-441.

Burri

Nikolic

Siegwart

(2016) The EuRoC micro aerial vehicle datasets. International Journal of Robotics Research 35(10): 1157–1163.

Bazin

Pollefeys

(2012) 3-line RANSAC for orthogonal vanishing point detection. In: 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, Vilamoura-Algarve, Portugal, 07-12 October, pp. 4282−4287. New York: IEEE.

Camposeco

Pollefeys

(2015) Using vanishing points to improve visual-inertial odometry. In: 2015 IEEE International Conference on Robotics and Automation, Seattle, WA, USA, 26-30 May, pp. 5219−5225. New York: IEEE.

Feng

Zhang

Peng

, et al. (2024) Improved monocular visual-inertial odometry with point and line features using adaptive line feature extraction. Multimedia Tools and Applications 83(1): 1481-1512.

Forster

Carlone

Dellaert

, et al. (2016) On-manifold preintegration for real-time visual–inertial odometry. IEEE Transactions on Robotics 33(1): 1–21.

Wang

, et al. (2020) PL-VINS: Real-time monocular visual-inertial SLAM with point and line features. arXiv preprint arXiv 07462.

10.

Gomez-Ojeda

Moreno

Zuñiga-Noël

PL-SLAM: A stereo SLAM system through the combination of points and line segments. IEEE Transactions on Robotics 35(3): 734-746.

11.

Galvez-Lopez

Tardos

(2011) Real-time loop detection with bags of binary words. In: 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems, San Francisco, CA, USA, 25-30 September, pp. 51−58. New York: IEEE.

12.

Hartley

Zisserman

(2003) Multiple view geometry in computer vision. Cambridge: Cambridge University Press.

13.

Zhao

Guo

, et al. (2018) Pl-VIO: Tightly-coupled monocular visual–inertial odometry using point and line features. Sensors 18(4): 1159.

14.

Huang

(2019) Visual-inertial navigation: A concise review. In: 2019 IEEE International Conference on Robotics and Automation, Montreal, Canada, 20-24 May, pp. 9572−9582. New York: IEEE.

15.

Kong

Zhang

, et al. (2015) Tightly-coupled stereo visual-inertial navigation using point and line features. Sensors 15(7): 12816−12833.

16.

Lee

Park

(2021) PLF-VINS: Real-time monocular visual-inertial SLAM with point-line fusion and parallel-line fusion. IEEE Robotics and Automation Letters 6(4): 7033−7040.

17.

Leutenegger

Lynen

Bosse

, et al. (2014) Keyframe-based visual-inertial odometry using nonlinear optimization. International Journal of Robotics Research 34(3): 314–334.

18.

Fang

Zhang

(2016) Visual servo regulation of wheeled mobile robots with an uncalibrated onboard camera. IEEE/ASME Transactions on Mechatronics 21(5): 2330−2342.

19.

Wang

, et al. (2024) Bi-level intelligent dynamic path planning for an UAV in low-altitude complex urban environment. Transactions of the Institute of Measurement and Control 46(1):39-57.

20.

Yan

Wang

, et al. (2023) A 3D LiDAR odometry for UGVs using coarse-to-fine deep scene flow estimation. Transactions of the Institute of Measurement and Control 45(2): 274-286.

21.

Pei

Xiang

, et al. (2022) P³-VINS: Tightly-coupled PPP/INS/visual SLAM based on optimization approach. IEEE Robotics and Automation Letters 7(3): 7021-7027.

22.

Lim

Kim

Jung

, et al. (2021) Avoiding degeneracy for monocular visual SLAM with point and line features. In: 2021 IEEE International Conference on Robotics and Automation, Xi’an, China, 30 May, pp. 11675−11681. New York: IEEE.

23.

Lim

Jeon

Myung

(2022) UV-SLAM: Unconstrained line-based SLAM using vanishing points for structural mapping. IEEE Robotics and Automation Letters 7(2): 1518−1525.

24.

Liu

Shi

, et al. (2021) PLC-VIO: Visual-Inertial Odometry Based on Point-Line Constraints. IEEE Transactions on Automation Science and Engineering 19(3): 1880-1897.

25.

Yaoy

, et al. (2017) 2-line exhaustive searching for real-time vanishing point estimation in manhattan world. In: 2017 IEEE Winter Conference on Applications of Computer Vision, Santa Rosa, CA, USA, 24-31 March, pp. 345–353. New York: IEEE.

26.

Mourikis

Roumeliotis

(2007) A multi-state constraint Kalman filter for vision-aided inertial navigation. In: 2007 IEEE International Conference on Robotics and Automation, Roma, Italy, 10-14 April, pp. 3565–3572. New York: IEEE.

27.

Qin

Shen

(2018) VINS-Mono: A robust and versatile monocular visual-inertial state estimator. IEEE Transactions on Robotics 34(4): 1004−1020.

28.

Qin

Pan

Cao

, et al. (2019) A general optimization-based framework for local odometry estimation with multiple sensors. arXiv preprint arXiv 1901.03638.

29.

Schubert

Goll

Demmel

, et al. (2018) The TUM VI benchmark for evaluating visual-inertial odometry. In: 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems, Madrid, Spain, 01-05 Oct, pp. 1680-1687. New York: IEEE.

30.

Shen

Michael

Kumar

(2015) Tightly-coupled monocular visual-inertial fusion for autonomous flight of rotorcraft MAVs. In: 2015 IEEE International Conference on Robotics and Automation, Seattle, WA, 26-30 May, pp. 5303–5310. New York: IEEE.

31.

Shi

(1994) Good features to track. In: 1994 Proceedings of IEEE Conference on Computer Vision and Pattern Recognition, Seattle, WA, USA, 21-23 June, pp. 593−600. New York: IEEE.

32.

Suárez

Buenaposada

Baumela

(2022) ELSED: Enhanced line SEgment drawing. arXiv 2108.03144.

33.

Tomasi

Kanade

(2003) Detection and tracking of point. International Journal of Computer Vision 9: 137-154.

34.

Von Gioi

Jakubowicz

Morel

, et al. (2010) LSD: A fast line segment detector with a false detection control. IEEE Transactions on Pattern Analysis and Machine Intelligence 32(4): 722–732.

35.

Wang

Chen

, et al. (2023) Cooperative target search and tracking for multi-UAVs based on control barrier functions. Transactions of the Institute of Measurement and Control 45(13):2582-2589.

36.

Wen

Tian

(2020) PLS-VIO: Stereo vision-inertial odometry based on point and line features. In: 2020 International Conference on High Performance Big Data and Intelligent Systems, Shenzhen, China, 23-23 May, pp. 1−7. New York: IEEE.

37.

Wang

, et al. (2022) Leveraging structural information to improve point line visual-inertial odometry. IEEE Robotics and Automation Letters 7(2): 3483−3490.

38.

Zhang

Yang

(2024) Monocular visual-inertial odometry leveraging point-line features with structural constraints. The Visual Computer 40(2): 647-661.

39.

Zhang

Koch

(2013) An efficient and robust line segment matching approach based on LBD descriptor and pairwise geometric consistency. Journal of Visual Communication and Image Representation 24(7): 794-805.

40.

Zheng

Tsai

Zhang

, et al. (2018) Trifo-VIO: Robust and efficient stereo visual inertial odometry using points and lines. In: 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems, Madrid, Spain, 01-05 October, pp. 3686−3693. New York: IEEE.

41.

Zou

Pei

, et al. (2019) StructVIO: visual-inertial odometry with structural regularity of man-made environments. IEEE Transactions on Robotics 35(4): 999−1013.