Stereo Plane SLAM Based on Intersecting Lines

We denote a plane feature as ${\pi}=\left(\bm{n}^{\top},d\right)^{\top}$, where $\bm{n}=\left(n_{x},n_{y},n_{z}\right)^{\top}$ is the unit normal vector representing the plane’s orientation and $d$ is the distance of the plane from the origin. We use the commonly used form $\bm{T}_{cw}\in{SE}(3)$ to represent a camera pose and $\bm{p}=\left(x,y,z,1\right)^{\top}$ to represent a 3D point. Therefore, $\bm{T}_{cw}\bm{p}_{w}$ transforms a point from the world to the camera coordinate system and $\bm{T}_{cw}^{-\top}{\pi}_{w}$ transforms a plane from the world to the camera coordinate system.

For lines, we only record their endpoints $(\bm{p}_{s},\bm{p}_{e})$ and unit direction vectors $\bm{n}_{l}$, which are enough to compute plane features.

A frame from a stereo camera consists of a left image $I_{l}$ and a right image $I_{r}$ . We use the Line Segment Detector [6] to extract line segments from both $I_{l}$ and $I_{r}$, and match them by the LBD descriptor [7]. The line matching is accurate and robust enough in one stereo frame. As shown in Fig. 2(a), the line segments are drawn in different colors, and the matched line segments are the same color in $I_{l}$ and $I_{r}$.

For every matched line segment in the left image $I_{l}$, we find corresponding points of its endpoints in the right image $I_{r}$ based on the fact that their row positions remain the same in $I_{l}$ and $I_{r}$ when using a parallel stereo camera. As shown in Fig. 2(b), matched endpoints are connected by transverse lines.

From the stereo matching of endpoints, we compute their 3D positions $\bm{p}$ based on disparities $\Delta u$. A line’s direction vector $\bm{n}_{l}$ is also defined by its two endpoints.

Before computing plane features, we need to check the relationship of lines. In three dimensions, two intersecting or parallel lines are on the same plane. For parallel lines, however, it is difficult to judge whether they are extracted from a real plane, and the planes computed from them are prone to bring large errors. Therefore, we only compute planes from intersecting lines.

To find intersecting lines quickly, we find the lines meeting the following conditions:

• •

  The angle between two lines is larger than the threshold ($10^{\circ}$ in our experiments)

• •

  The distance between their central points is smaller than the length of the line.

• •

  The four endpoints of these two lines lie on the same plane.

The central points $\bm{p}_{c}$ in the second condition are computed from lines’ endpoints $\bm{p}_{s}$ and $\bm{p}_{e}$. From the first two conditions, we actually find unparallel lines that are close with each other. We compute the plane’s normal vector by the cross product of the lines’ direction vectors,

Using the plane’s normal $\bm{n}_{\pi}$ and four endpoints $\bm{p}_{k}$ $(k=1,2,3,4)$, we then compute four different plane coefficients $d_{k}$,

The distance among them is:

If $D$ is smaller than the threshold (5 cm in our experiments), these two lines meet the third condition and the plane coefficients ${\pi}=\left(\bm{n}_{\pi}^{\top},\bar{d_{k}}\right)^{\top}$ are also computed, $\bar{d_{k}}$ here is the arithmetic average of $d_{k}$. Sometimes the computed planes may not be real planes, such as the plane from the lines of a doorframe. But such planes are also stable enough and provide accurate constraints, and therefore we treat them as real planes.

Under these conditions, we compute as many planes as possible at first. We will validate the computed planes in the frame tracking and label inaccurate planes as invalid, which is described in Sec. IV-E.

The features extracted and computed from the images of Fig. 2 is shown in Fig. 1. The black points are feature points extracted from the images. The red lines are the line segments extracted from the images, whose 3D positions are computed from the matched endpoints. Notice that we do not use these line segments in our SLAM system, and we draw them in the figure just to show the process of computing plane features. We draw the plane features by expanding corresponding intersecting lines and they are drawn in different colors. The red plane in the middle of the figure seems not correct because of errors from line segments, and we label this plane as invalid in the validation. But other planes are correct and are used as valid landmarks in our SLAM system.

The pipeline of our proposed SLAM system is illustrated in Fig. 3. It can be divided to three parts, frame processing, pose tracking and mapping. We do not add loop closure now, which will be a part of our future work.

In stereo frame processing, we extract feature points and line segments from both left and right images, and match these features based on descriptors. Then we can compute plane features using the method described in Sec. III.

In pose tracking, every camera pose is estimated based on matched valid features. The camera pose is firstly estimated from the last keyframe and then optimized in the local map.

In mapping, points and planes are constructed and saved in the map. To achieve more accurate estimation, a local map optimization is performed.

SLAM is commonly formulated as a nonlinear least squares optimization problem [1], and the bundle adjustment (BA) is commonly used for point features [2]. Like points, we also design the optimization formulation for plane features. In our SLAM system, we denote a set of camera poses, point features and plane features as $C=\{{c}_{i}\}$, $P=\{{p}_{j}\}$, $\Pi=\{{\pi}_{k}\}$ respectively, then the optimization problem can be formulated as:

${e}\left({c},{p}\right)$, ${e}\left({c},{\pi}\right)$ represent the camera-point and camera-plane measurement errors respectively. $\|\bm{x}\|_{{\Sigma}}^{2}$ is the Mahalanobis distance, which equals $\bm{x}^{\top}{\Sigma}^{-1}\bm{x}$. ${\Sigma}$ is the corresponding covariance matrix, it is set based on the measurement uncertainty (such as $2^{\circ}$ for plane angles in our experiments).

The optimization problem can be solved using Levenberg-Marquardt or Gauss-Newton implemented in g2o [23].

We try to match every observation of points and planes with the landmarks in the map. This process is defined as data association. Robust data association is also necessary for accurate estimation results. Point features are commonly matched by their descriptors.

To match plane features, previous works [5, 20] depend on the plane parameters $\bm{n}^{\top}$ and $d_{\pi}$ directly. This method is simple and works well for small scenes. But it needs accurate estimation of camera poses, and $d_{\pi}$ may fluctuate largely because of errors. To refine the data association of planes, we use the distances of endpoints instead.

After computing planes from intersecting lines, we save the endpoints of the lines. To match plane features, we compute the average distance $\bar{d}_{ep}$ from the endpoints to plane landmarks. Unlike $d_{\pi}$, the fluctuation of $\bar{d}_{ep}$ is relatively small. If $\bar{d}_{ep}$ is smaller than the threshold (6 cm in our experiments) and the angle between plane normal vectors is also smaller than the threshold ($12^{\circ}$ in our experiments), the plane landmark is matched with the corresponding plane observation.

We use the same method to select keyframes as in ORB-SLAM [2]. From the keyframe, every unmatched plane feature is used to create a new plane landmark. But these plane landmarks may be inaccurate, and therefore they are labelled invalid. The planes are changed to be valid only if they are observed in sufficient frames (3 keyframes in our experiments). Only valid plane features are used to estimate camera poses and added into the optimization framework (Eq. (4)).

The EuRoC dataset contains stereo sequences recorded from a micro aerial vehicle flying around in three different indoor environments, including an industrial machine hall and two Vicon rooms. These scenes contain man-made objects and structures, which are easy to extract line segments and compute plane features. The sequences are classified as easy, medium, and difficult according to the speed, illumination, and scene texture. The dataset also provides ground truth from the laser tracking system and the motion capture system.

Table I shows the comparison of estimation results of different SLAM systems. Here we use the absolute translation root mean square error (RMSE) to evaluate the estimation results. The smallest error for each sequence is labelled as a bold number. It is clear that our system outperforms the stereo ORB-SLAM2 in these sequences. The computed plane features bring more constraints to estimate camera poses, and these constrains are reasonable in such indoor environments.

The line-based SLAM system also performs better than ORB-SLAM2 in these sequences, and gets comparable results with ours. Although using line segments directly adds more information to the SLAM system, some line segments are not accurate and the data association is difficult when the camera view changes, as shown in Fig. 4. A single line in the scene may be constructed as several segments because of the failure of data association. By computing planes from line segments, our system may lose some information but filters out inaccurate line segments, and the data association of planes is easier between frames. The estimation results of our system are better in industrial machine hall sequences(MH*). We find the objects and structure in the industrial machine hall have clear and sharp edges, which benefits the line extraction and plane calculation. But in the Vicon room (V1* and V2*), there are more errors of the lines extracted from the soft cushions and curtains.

An example of the built map from our system is shown in Fig. 5. The map consists of points, planes and camera poses. The plane features enrich the map and help to reflect the real scene structures clearly. It demonstrates the plane features are computed from the main objects and structure, such as walls, cushions and etc. The planes from the wall and the ground are easily matched even in frames of a large distance. Although we try to compute accurate planes and filter out those with large errors, some inaccurate planes still exist in the map.

The KITTI dataset contains stereo sequences recorded from an autonomous driving platform driving in urban and highway environments. The ground truth is given by the GPS/IMU localization unit. Although our SLAM system is more suitable for indoor environments, the planes can also be computed from man-made structures (such as houses and roads) in these sequences.

Table II shows the comparison of estimation results in the KITTI dataset. We again calculate the RMSE of these sequences and label the smallest errors as bold numbers. Our system also gets better estimation results in most of the sequences. The performance of ORB-SLAM using only point features is good enough, and our system only gets similar results in some sequences. This is because the camera moves much faster in these sequences, and thus planes are only matched in a few frames and fewer valid planes are created. Fig. 6 shows the examples of the estimated trajectories compared with the ground truth and our results are closer to the ground truth.

The line-based SLAM system also performs better in some sequences. But in other sequences, it gets even worse results, such as the Sequence 01, 02, and 07. This is because the extracted lines have large errors and are difficult to match when the camera moves quickly.

A part of the built map in the Sequence 00 is shown in Fig. 7. The plane features are computed from roads and walls. It is clear the drift error is small.

We record the average processing time of the main parts of the proposed system, as shown in Table III. In the table, feature processing includes point extraction, line extraction, stereo matching, and plane computation; feature tracking includes landmark matching and camera pose estimation. Frame tracking is the whole process for tracking a new coming stereo frame. The most time-consuming part is the feature processing, specifically point and line extraction. Because the images from KITTI sequences are larger, feature processing needs more time.

We also record the average processing time of the line-based SLAM system [18]. The average time for its frame tracking are $81.8247ms$ in the EuRoC sequences and $105.347ms$ in the KITTI sequences. It is clear that our system is faster than the SLAM system using line segments directly. In the line-based system, many line segments are built with large errors, and they are hard to be matched correctly, as shown in Fig. 4. But all of these features are added into the optimization function, and therefore it needs more time to estimate the optimal results.