# A Visual SLAM Approach to Deeply Explore the Spatial Constraints in Indoor Environment Based on the Manhattan Hypothesis

Authors: Xiaoguo Zhang; Shangzhe Li; Qihan Liu; Pengfei Liu; Qing Wang
Journal: Journal of Sensors
 (2023)

---

## Abstract

It is a major challenge for a visual SLAM system to maintain a highly precise and robust mapping and localization ability in an environment with low texture. In this paper, a bundle adjustment method for deeply exploring the spatial constraints based on the Manhattan hypothesis is proposed. First, an energy function for filtering the feature set and segmenting the point cloud is given, and a semantic graph is formed with the minimum energy. Then, a novel factor graph for back-end bundle adjustment is designed to include points, edges, planes, and spatial constraints to improve the stability and positioning accuracy of the system. After that, the corresponding error items of those spatial constraints for bundle adjustment are designed according to the Manhattan hypothesis. Finally, comparison tests are designed to evaluate the performance of the given approach using TUM RGB-D datasets in different scenes. Test results show that the proposed method provides better positioning ability, with the positioning accuracy improved by 18.07% on average compared with that of existing mainstream approaches.


---

## Body

## 1. Introduction

Recently, visual simultaneous localization and mapping (V-SLAM) technology has received increasing attention in fields such as environment reconstruction and indoor navigation [1–4]. For indoor navigation, a structured scene generally refers to a place where there are many planes, inadequate texture information, or indistinct discrimination [5]. Under structured scenes with low texture, most existing V-SLAM systems have problems, such as mismatching and losing-matching, which can easily lead to tracking failure and low positioning accuracy [6].Motivated by this problem, many studies [7–11] have been conducted to improve SLAM performance by using structural features, which mainly include line features and plane features. In addition, a variety of feature extraction methods and back-end optimization methods have been proposed. However, most of the front-end feature extraction processes focus on extracting features more accurately, which means they try to mitigate the no-sufficient-constraints issue of the feature-point method by thoroughly using structure features. By either enhancing the accuracy and precision of feature extraction in the front end or by including error items corresponding to the spatial constraints in the objective functions in the back end, they increase the system’s precision and robustness. However, they lack an efficient filtering mechanism, and the spatial constraints among those features are not fully explored in the back-end bundle adjustment (BA) process.This article provides a novel bundle adjustment method that fully takes advantage of edge features, plane features, and the spatial constraints among them. In the front end, plane features and related contour edge features are extracted based on the depth image, and the spatial constraints among them are identified. For the extracted plane and the edge features, an energy function is designed to filter the feature set and segment the point cloud, and a semantic graph is generated with the minimum energy. In the back end, a novel factor graph for bundle adjustment is designed to include points, edges, planes, and spatial constraints to improve the stability and positioning accuracy of the system. The corresponding error items of those spatial constraints for bundle adjustment are designed according to the Manhattan hypothesis. The main contributions of this paper are as follows:(1)
An energy function considering both the internal and external constraints of features is proposed to filter outlier structural features(2)
A bundle adjustment method using a novel factor graph fully considering the reprojection errors of points, edges, and their spatial constraints is provided(3)
Comparison tests are designed and run, and the results show that our approach achieves a higher accuracy positioning performance compared with the existing mainstream method


## 2. Related Works

Recently, an increasing number of studies have attempted to improve the performance of SLAM systems in low-texture environments by using structural features, including line features and plane features from various vision sensors. Arndt et al. [7] used a monocular camera to extract plane features in the first step and utilized the factor graph to optimize the SLAM system using the plane, the points in the plane, and the regular 3D points. In [8], a semantic planar SLAM system was presented that improved pose estimation and mapping using cues from an instance planar segmentation network. The proposed network exploits spatial coherence to tackle the practical issue when the output of a trained CNN (convolutional neural network) is inaccurate. Clustering and image segmentation [9] were also adopted and proved to detect planes effectively. The above methods added structural features to the SLAM system by using a monocular camera.Since depth images have great advantages in structural feature extraction, more approaches using RGB-D cameras have been proposed. Ozaki and Kuroda [10] proposed using global plane features as landmarks in the extended Kalman filter (EKF) to reduce the cumulative error of the estimation; however, it did not achieve a bundle adjustment and could only be adopted for some small scenes. Wang et al. [11] extracted and matched superpixel information from two images. Then, they gave a novel multimodel RANSAC method to estimate multiple homographies from superpixels and identify the matching planes. However, these methods extracted structural features and only used the internal information of the features for matching, without fully taking advantage of the constraints among them.Many methods have been developed to add structural features to back-end optimization, and various residual expressions have been proposed. By using plane features instead of point features and using the associated planes based on edge features to construct the constraint of the factor graphs, a method for estimating the sensor pose in structural and low-texture environments was proposed by Guo et al. [12]. Zhang et al. [13] used the intersecting lines to calculate the plane parameters and added the plane factor to the back-end optimization to obtain more accurate point clouds using a stereo camera. The aforementioned methods had better performances than line-based SLAM systems. Ma and Liang [14] proposed an RGB-D SLAM method combining point and line features and added those point and line features to the global map and the local BA. However, they put the points on the line to the back-end optimization without using them in the global BA. Planar Bundle Adjustment (PBA) [15] jointly optimizes the pose of the depth sensor and plane parameters for 3D reconstruction to reduce drift and improve the quality of the map, and such an algorithm using the point-to-plane cost significantly reduces the computational cost. With the philosophy of visual saliency and attention starting to gain popularity in the robotics community, Wang et al. [16] proposed a saliency model to predict the saliency map and used the value of the saliency map as the weight of the feature points to improve BA. To prominent robust and rich semantic features, the salient bundle adjustment (SBA) [17] algorithm computes the saliency map by considering both geometric and semantic information and uses the value of the saliency prediction map as the weight of the feature points. PL-SLAM [18] combined both points and line segments and leveraged both points and line segments at all the instances of the process. The above methods have added structural features to the back end but without considering the structural errors and drifts in those features. PLP-SLAM [19] demonstrated a system that utilizes point and line cloud for robust camera localization with an embedded piece-wise planar reconstruction (PPR) module which in all provides a structural map. If an unwanted error item is added to the back-end optimization, the performance of the aforementioned systems could be seriously affected.The Manhattan hypothesis is applicable to most indoor environments and can be used as a reference to compute pairwise spatial transformations. Thus, such spatial constraints can be added to the back-end SLAM optimization. In Zhang’s work, a convolutional neural network was used in combination with weak Manhattan hypothesis clustering to predict the surface normal in real time from each input RGB image; then, the translations and poses were estimated using the point and line features [20]. Li et al. [21] proposed a framework which can robustly utilize the Manhattan world structure and a method to detect Manhattan Frames (MF) directly from planes, allowing the system to model the scene as a mixture of Manhattan Frames. Company-Corcoles et al. [22], after combining points, lines, and the Manhattan axis of the scene, calculated the reprojection error of points and lines to optimize the camera pose through local map optimization. Li et al. [23] proposed a decoupling-refinement method based on points, lines, and planes, as well as the use of Manhattan relationships in an additional pose refinement module. Considering the spatial constraints of the Manhattan and Atlanta worlds, a novel tracking-by-detection scheme [24] that decouples the camera rotation by tracking structural regularities and estimates the underlying unknown structural regularities was proposed. SP-SLAM [25] generates point clouds from depth images to extract planes and adds parallel and perpendicular constraints for planes, which helps reduce drift errors in indoor environments. Such a method constructs constraints on the factor graph by using planar features instead of point features and using association planes based on edges; nevertheless, the mismatch among plane features is inevitable, which could greatly downgrade the system performance.Overall, most existing works have focused on how to extract structural features for performance improvement, but they lack an efficient feature filtering mechanism to remove noise features. Meanwhile, when structural features are added to the back-end optimization, the constraints among the features have not been fully explored. Aiming at the problem, this paper uses an energy function to filter structural features and proposes a new bundle adjustment method based on factor graph optimization by deeply exploring the spatial characteristics and constraints in the indoor environment according to the Manhattan hypothesis. Test results show that the proposed method provides better positioning ability with positioning accuracy that improved by 18.07% on average compared with existing mainstream approaches.


## 3. Proposed Method

### 3.1. Overview

Our algorithm is implemented based on the ORB-SLAM2 [26]. The RGB-D system and an overview of the test system are shown in Figure 1, which includes three parallel threads:
(1)
A tracking thread, which is used to locate the camera and decide whether to insert a new keyframe. In the thread, point, edge, and plane features are extracted from each frame to estimate the camera pose(2)
A local mapping thread, which is used to process new keyframes and perform local BA to optimize the camera pose. In addition, this thread will create new map points, map edges, and map planes together while removing redundant keyframes(3)
A loop closing thread, which is mainly responsible for loop detection (using a DBOW bag of words) and global map optimization to minimize the overall errorFigure 1
The framework of the proposed visual SLAM.To evaluate the performance of the proposed algorithm, comparison tests are conducted using the TUM RGB-D dataset, which supports scenes with different texture features and structural landmarks.


### 3.2. Edge and Plane Feature

In this paper, the pinhole model is used to map points in the three-dimensional real world to two-dimensional image planes as follows:(1)uv1=1dfx0cx0fycy001xyz,where u=u,vT is the pixel coordinates corresponding to the depth value d in the depth image and 3D point p=x,y,zT, fx,fy is the focal length of the camera, and cx,cy is the camera coordinate.We letTkw represent the pose transformation matrix of the kth frame relative to the world coordinate system w, where Tkw∈SE3. Then, the 3D point Pk in the kth frame can be rigidly transformed to the world coordinate system using the equation as follows:
(2)Pk=TkwPw.l=nlT,plTT=nx,ny,nz,px,py,pzT is used to parametrize edges, where nlT is the unit direction vector of the edge and plT is a boundary point on this edge. Every point p lies on the edge and is subject to the equation as follows:
(3)nlTp−pl=0.Therefore, an edgelw in the camera frame can be transformed into the world frame as follows:
(4)lk=Tkwlw.Generally, planes can be parametrized by the vector formπ=nπT,dT, where nπ=nx,ny,nzT is the unit vector representing the orientation of the plane and d is the distance from the plane to the origin. Every point p in the plane is subject as follows:
(5)nπTp+d=0.Consequently, a single plane in the camera frame can be transformed into the world frame as follows:(6)πk=Tkwπw.The RGB-D camera provides paired RGB images and depth images. By using the depth image, this paper extracts plane features from the point cloud and uses edge features to segment the plane. The extracted edges and planes can be observed by adjacent frames. Finally, these features are involved in back-end optimization as factors in the pose graph to improve the system positioning and mapping accuracy.Structural features can generally be found in the point cloud generated from the depth image, which can be used to support the rapid segmentation of the planar point cloud [27]. In the above process, the contour of the plane can also be obtained. Although the plane represented by π=nπT,dT is infinite, in the real structured scene, the plane is finite and requires boundary edges to constrain. Therefore, the contour is used as an important basis for constraining and calculating planes.In the front end, the traditional LSD algorithm is commonly used for edge feature extraction and matching. However, for scenes with low texture, the difference between different planes is not obvious, and the contours are not highly distinguished, which leads to more messy edges in the image. In structured scenes, such as corridors, extracting contour edges can effectively reduce both invalid edge features and mismatches, which could be used to improve the robustness and positioning accuracy of the system.In this paper, the random sample consensus (RANSAC) algorithm is used to extract the contours of the plane features, and when there are enough inliers (more than 20% of the contour points are inliers in the experiment), the boundary edge is considered valid. RANSAC is also used to filter and merge messy straight edges and eliminate invalid straight edges at the same time.


### 3.3. An Energy Function Based on the Manhattan Hypothesis

The energy function is frequently used in image and point cloud segmentation to describe the degree of difference between different semantic information objects. Many semantic segmentation or deep learning algorithms, such as PointNet and RBM (restricted Boltzmann machine), are based on energy functions. Through energy clustering with the given threshold setting, objects with different semantic information are assigned to various categories [28].After segmenting the plane candidate setPπ1,π2,⋯,πN of the structured scene, it is essential to effectively filter those planes affecting the accuracy of the point cloud cluster in the global map. Here, a point cloud segmentation optimization algorithm based on the energy function according to the Manhattan hypothesis is given. Considering that there are many regular plane geometric constraints in structured scenes, such as corridors, walls, and desks indoors, and that most of the plane structures are perpendicular or parallel to each other, an energy function can be naturally defined by these constraints and could be used to filter the final point cloud segmentation result C=c1,c2,⋯,cM by minimizing the energy function.The energy function of point cloud semantic segmentation mainly includes first-order data items and second-order spatial constraint items, where the data item represents the fitting degree of the point cloud in the plane setP, and the spatial constraint item represents the geometric constraint relationship (perpendicular or parallel) between the planes in the structured scene. Then, the energy function is written as follows:
(7)EP=∑πn∈PE1πn+∑πm,πn∈PE2πm,πn,C=argminEP,where EP represents the total energy function, C is the optimized final point cloud cluster set, E1 is the first-order data item, and E2 represents the spatial constraint-related item.ForE1, the candidate set Pπ1,π2,⋯,πN is determined using the distance between the plane’s centroid and the structured scene point cloud classified to the plane. The fitting degree between the plane candidate set and the point cloud can be determined; therefore, those candidates with a low degree can be deleted. The point cloud set corresponding to the candidate plane in the scene can be expressed as follows:
(8)Cm=pidπpG,pi<σ1,where dπpG,pi is the distance from point cloud pi to centroid πpG of a certain candidate plane, Np is the point cloud set corresponding to the candidate plane in the scene, and σ1 is the distance threshold (σ1 is set to 20 cm). The more point clouds with distance from the candidate plane within the threshold are classified as planes; the more attribution point clouds better represent the plane fit. Then, the data item E1 can be described as follows:
(9)E1πn=1−Npλ,Np<λ,expλ−Npλ,Np≥λ,where λ is the threshold of the degree of fit between the plane candidate set and the point cloud, and it is set as 40 in our test.Since there are many mutually perpendicular or parallel planes in the structured scene, if they can be transferred to an item in the energy function, the optimization result will be close to the selection of clustering point clouds into a set of candidate planes that are parallel or perpendicular to each other. The geometric structure constraint-related itemE2 is defined as follows:
(10)E2πm,πn=1−exp−φ′πm,πnσ2,φ′πm,πn=minφπm,πn,π2−φπm,πn,where φπm,πn is the angle between planes πm and πn in the set of candidate planes, φ′πm,πn is the angle difference between the two planes that are parallel or perpendicular to each other, and σ2 represents the preset increase step of the angle (σ2 is set 10°). When the two planes are sufficiently parallel or perpendicular to each other (parallel is set to 10° and perpendicular is set to 80° in the experiment), the value of φ′πm,πn is 0, and the added energy term E2πm,πn is also 0. Moreover, the three-dimensional energy of the energy function will not increase, and the plane may exist in the final plane segmentation set. If the correlation between the geometric constraints of the two planes is lower, neither perpendicular nor parallel, then the energy value will be larger, and the algorithm will tend to eliminate the plane.Given the candidate setPπ1,π2,⋯,πN, each plane is only selected or eliminated by one category, and thus, the result of the energy function is as follows:
(11)EP=∑n=1NE1Inbn+∑pm,pn∈PE2Im,Inbmbn,where B=b1,b2,⋯,bN is a binary set to represent whether the plane in the candidate set Pπ1,π2,⋯,πN is selected, bn=1 means πn is selected for the final set, E1 represents the first-order data item, and E2 represents the geometric structure constraint-related items. Therefore, the simplified form of the energy function can be written as follows:
(12)EB=BTU+BTMB,B∈0,1,where U is the first-order data item of the energy function and M=mij∈R represents the correlation coefficient of two candidate planes, which is a symmetric matrix. When two planes are not correlated, then mij=0.This paper introduces an iterative trust-region optimization algorithm to solve the energy function. In the process, an approximate model of the optimization problem is constructed in the trust region of the current resultBt; then, the final set is obtained by optimizing the global model in the trust region. Meanwhile, the trust region needs to be based on the current approximation during the iteration. Thus, the above equation is expressed as the sum of a positive correlation term and a negative correlation term as follows:
(13)EB=ENB+EPB,ENB=BTU+BTM−B,EPB=BTM+B,where M− and M+ represent the part of the correlation coefficient mij−≤0 and mij+≥0 in the symmetric matrix 𝑀, respectively. Given the current result Bt, the energy function EB can be approximated in the trust region as follows:
(14)EtB=ENB+BTUt+c,where Ut=2M+Bt and Ut+c are first-order Taylor expansions from EPB, and we can solve the energy function EtB in the trust region σt as follows:
(15)B∗=argminB−Bt<σtEtB.The above energy function is transformed by solving the subproblem of unconstrained Lagrangian optimization in the trust region. The subproblem equation of the transformed trust region is as follows:(16)LtB=EtB+λt·B−Bt,where λt decides the size of the trust region. The algorithm updates λt through each iteration adaptively according to the inversely proportional relationship between λt and σt, consequently updating the size of the trust region. After obtaining the candidate result B∗, the quality of the candidate result can be evaluated according to the ratio between the actual energy reduction R and the predicted energy reduction P of the energy function. The quality of the approximate function is evaluated by calculating R/P. If R/P exceeds the given threshold σ1, the optimal result is updated and the range of the trust region λt is adjusted as follows:
(17)λt+1⟵λtα,RP>σ2,λt·α,RP≤σ2.To filter the candidate results, the threshold parameterλt is set to 1, σ2 is set to 0.25, and α is set to 2. When the decreased rate of the energy function exceeds the threshold, the algorithm expands the range of the trust region λt. By iteratively solving the subproblems of unconstrained Lagrangian optimization under the trust region, the final filtered set C=c1,c2,⋯,cM is obtained.Similar to the process of constructing the energy function of planes,Ql1,l2,⋯,lN is defined as the edge candidate set and F=f1,f2,⋯,fM as the final point cloud segmentation result. According to the Manhattan hypothesis, the energy function for the edges can be defined as follows:
(18)EQ=∑ln∈QE1ln+∑lm,ln∈QE2lm,ln,F=argminEQ.ForE1, by calculating the distance between the geometric centre of the candidate edge and the structured scene point cloud classified to the edge, the degree of the fitting is obtained to filter the set Ql1,l2,⋯,lN. The point cloud set of candidate edges is as follows:
(19)Fm=pidlpG,pi<ε1,where dlpG,pi is the distance from point cloud pi to geometric centre lpG of a certain candidate edge, Np is the point cloud set corresponding to the candidate edge in the scene, and ε1 is the distance threshold. The data item E1 is expressed as follows:
(20)E1ln=1−Npλ,Np<λ,expλ−Npλ,Np≥λ,where λ is the threshold of the degree of fit between the edge candidate set and the point cloud and it is set as 10 in our test.Since the contour vectors of plane features are mostly parallel or perpendicular to each other, the geometric structure constraint itemE2 is added to the energy function in the candidate edge filtering process. Similar to plane geometric structure constraints, the geometric structure constraint related to item E2 is expressed as follows:
(21)E2lm,ln=1−exp−φ′lm,lnε2,φ′lm,ln=minφlm,ln,π2−φlm,ln,where φlm,ln is the vector angle between lm and ln in the set of candidate edges, φ′lm,ln is the two candidate edges that should match the angle between the parallel or perpendicular set constraints, and ε2 is the set angle increase step length.Similar to the process of solving the energy function of planes, the inappropriate edges are eliminated, and the final candidate set is obtained.


### 3.4. Back-End Graph Optimization Using a Point-Edge-Plane-Based Pose Graph

Factor graphs are frequently used in SLAM systems to express the back-end optimization problem since they are more suitable for performing reasoning than Bayesian networks. Under the assumption of the Gaussian noise model, this problem is equivalent to solving the nonlinear least-squares problem.


#### 3.4.1. Graph Optimization Structure Overview

In this paper, a novel factor graph for back-end bundle adjustment is designed to include points, edges, planes, and their spatial constraints to improve the stability and positioning accuracy of the system. Figure2 includes three factors: (a) point landmarks, (b) edge landmarks, and (c) plane landmarks. Based on the Manhattan hypothesis, there are structural relationship constraints between the plane landmark and the edge landmark. Therefore, there are 5 residual values in the graph optimization structure of this article: (a) point landmark reprojection error, (b) plane landmark reprojection error, (c) edge landmark reprojection error, (d) plane landmark structural error, and (e) edge landmark structural error.Figure 2
A novel factor graph for deeply exploring the spatial constraints based on the Manhattan hypothesis.


#### 3.4.2. Error Items of Point Landmark Factors

The error item of a point landmark can be calculated by reprojecting it into pixel coordinates as the observation point coordinate to obtain the reprojection error, which can be written as follows:(22)eppw,Tcw=z−ρTcwpw,where ep is the reprojection error, ρp is the pixel coordinates of the point landmark, and z≜us,vsT are the pixel coordinates of the observation point. We let zij be the pixel coordinates of landmark pwj observed at pose Tcwi, and the overall error items of all observed point landmarks during a certain period can be described by the equation as follows:
(23)Ep=12∑i=1m∑j=1nepTcwjpwj2=12∑i=1m∑j=1nzij−ρTcwipwj2.


#### 3.4.3. Error Items of Plane Landmark Factors

Generally, there are six degrees of freedom for a rigid object in three-dimensional space: translations in thex, y, and s directions and rotations around the x‐y‐s axis. However, when the rigid body’s motion is limited to the plane, only two degrees of freedom of the translational motion and the rotation axis are fixed. Therefore, this paper adopts the minimum parametrized plane expression τ=φ,ψ,d to avoid the hyperparameter problem as follows:
(24)τ=qπ=φ=tan−1nynx,ψ=sin−1nz,dT,where φ∈−π,π and ψ∈−π,π are the azimuth and elevation angles of the normal, respectively. The error item of a single observation plane can be written as follows:
(25)eπ1πw,Tcw=qπ−qTcw−Tπw,where qπ is the minimized parameter coordinate of the observation plane. After considering the observations at other moments, we let qπij be the parameter coordinates of plane πwj observed at pose Tcwi. The error item of the observation part can be written as follows:
(26)Eπ1=12∑i=1m∑j=1neππwj,Tcwi2=12∑i=1m∑j=1nqπij−qTcwi−Tπwj2.In addition, according to the Manhattan hypothesis, there are parallel and perpendicular structural relationships between plane landmarks; therefore, the error item for plane landmark factors is given as follows:(27)eπ2πw,Tcw=mineπp,eπv=minqnn−qnRcwnw,qnRtgn−qnRcwnw,where eπp is the parallel constraint error, eπv is the perpendicular constraint error, qn refers to the azimuth and elevation angle, n is the observation plane normal, Rcw is the rotation matrix part of the pose transformation matrix Tcw, and Rtg is an additional rotation matrix to rotate the normal to the same direction. Considering the observations at other moments, we let qnnij be the normal of plane πwj observed at pose Tcwi, and the error item of the structural constraint part can be written as follows:
(28)Eπ2=12∑i=1m∑j=1nminqnnij−qnRcwinwj,qnRtgnij−qnRcwinwj2.


#### 3.4.4. Error Items of Edge Landmark Factors

A straight line in 3-D space has only four degrees of freedom since nothing is changed when a straight line rotates around itself or translates along its own direction. In this paper, the minimum parametrized edge expressionε=φ,ψ,ulTT is used to avoid the hyperparameter problem as follows:
(29)ε=sπ=φ=tan−1nynx,φ=sin−1nz,ρplTT.The error item for a single observation edge can be defined as follows:(30)el1lw,Tcw=sl−sTcw−Tlw,where sl is the minimized parameter coordinate of the edge of the current observation. Additionally, we let slij be the parameter coordinate of edge lwj observed at pose Tcwi; then, the error item of the observation edge landmarks during a certain time can be described as follows:
(31)El1=12∑i=1m∑j=1nellwj,Tcwi2=12∑i=1m∑j=1nslij−sTcwi−Tlwj2.Since there are parallel or perpendicular structural relationships among edge landmarks, the error item considering such constraints can be written as follows:(32)el2πw,Tcw=minelp,elv=minsnn−snRcwnw,snRtgn−snRcwnw,where elp is the parallel constraint error, elv is the perpendicular constraint error, and n is the observed edge direction vector. Considering the observations during certain times, snnij is assumed to be the direction vector of edge lwj observed at pose Tcwi, and the error item of the structural constraint part can be described as follows:
(33)El2=12∑i=1m∑j=1nminsnnij−snRcwinwj,snRtgnij−snRcwinwj2.


### 3.5. Pose Optimization

In Section3.4, several error items are introduced for back-end graph optimization based on the Manhattan hypothesis. In the tracking process, the system will optimize using point, edge, and plane features and the relationships among them using a sliding window through local BA and detect loop closure. When a loop closure is detected, the system will terminate local optimization and perform global optimization.By using the aforementioned error terms, the camera pose in the local map is optimized by local BA by minimizing the reprojection error of those point, edge, and plane features seen in these keyframes, as well as the structural error of those planes and the edges in the sliding window. The features observed by all keyframes outside the sliding window also contribute to the error item, but they are kept fixed in the optimization. Finally, the optimization problem can be transformed into the optimization problem as follows:(34)Tcw=argminHpEp+Hπ1Eπ1+Hπ2Eπ2+Hl1El1+Hl2El2,where Hx is the Huber robust cost function and E is the error term introduced above. To resolve the aforementioned optimization problem, the Levenberg-Marquardt method and g2o [29] can be used.


## 3.1. Overview

Our algorithm is implemented based on the ORB-SLAM2 [26]. The RGB-D system and an overview of the test system are shown in Figure 1, which includes three parallel threads:
(1)
A tracking thread, which is used to locate the camera and decide whether to insert a new keyframe. In the thread, point, edge, and plane features are extracted from each frame to estimate the camera pose(2)
A local mapping thread, which is used to process new keyframes and perform local BA to optimize the camera pose. In addition, this thread will create new map points, map edges, and map planes together while removing redundant keyframes(3)
A loop closing thread, which is mainly responsible for loop detection (using a DBOW bag of words) and global map optimization to minimize the overall errorFigure 1
The framework of the proposed visual SLAM.To evaluate the performance of the proposed algorithm, comparison tests are conducted using the TUM RGB-D dataset, which supports scenes with different texture features and structural landmarks.


## 3.2. Edge and Plane Feature

In this paper, the pinhole model is used to map points in the three-dimensional real world to two-dimensional image planes as follows:(1)uv1=1dfx0cx0fycy001xyz,where u=u,vT is the pixel coordinates corresponding to the depth value d in the depth image and 3D point p=x,y,zT, fx,fy is the focal length of the camera, and cx,cy is the camera coordinate.We letTkw represent the pose transformation matrix of the kth frame relative to the world coordinate system w, where Tkw∈SE3. Then, the 3D point Pk in the kth frame can be rigidly transformed to the world coordinate system using the equation as follows:
(2)Pk=TkwPw.l=nlT,plTT=nx,ny,nz,px,py,pzT is used to parametrize edges, where nlT is the unit direction vector of the edge and plT is a boundary point on this edge. Every point p lies on the edge and is subject to the equation as follows:
(3)nlTp−pl=0.Therefore, an edgelw in the camera frame can be transformed into the world frame as follows:
(4)lk=Tkwlw.Generally, planes can be parametrized by the vector formπ=nπT,dT, where nπ=nx,ny,nzT is the unit vector representing the orientation of the plane and d is the distance from the plane to the origin. Every point p in the plane is subject as follows:
(5)nπTp+d=0.Consequently, a single plane in the camera frame can be transformed into the world frame as follows:(6)πk=Tkwπw.The RGB-D camera provides paired RGB images and depth images. By using the depth image, this paper extracts plane features from the point cloud and uses edge features to segment the plane. The extracted edges and planes can be observed by adjacent frames. Finally, these features are involved in back-end optimization as factors in the pose graph to improve the system positioning and mapping accuracy.Structural features can generally be found in the point cloud generated from the depth image, which can be used to support the rapid segmentation of the planar point cloud [27]. In the above process, the contour of the plane can also be obtained. Although the plane represented by π=nπT,dT is infinite, in the real structured scene, the plane is finite and requires boundary edges to constrain. Therefore, the contour is used as an important basis for constraining and calculating planes.In the front end, the traditional LSD algorithm is commonly used for edge feature extraction and matching. However, for scenes with low texture, the difference between different planes is not obvious, and the contours are not highly distinguished, which leads to more messy edges in the image. In structured scenes, such as corridors, extracting contour edges can effectively reduce both invalid edge features and mismatches, which could be used to improve the robustness and positioning accuracy of the system.In this paper, the random sample consensus (RANSAC) algorithm is used to extract the contours of the plane features, and when there are enough inliers (more than 20% of the contour points are inliers in the experiment), the boundary edge is considered valid. RANSAC is also used to filter and merge messy straight edges and eliminate invalid straight edges at the same time.


## 3.3. An Energy Function Based on the Manhattan Hypothesis

The energy function is frequently used in image and point cloud segmentation to describe the degree of difference between different semantic information objects. Many semantic segmentation or deep learning algorithms, such as PointNet and RBM (restricted Boltzmann machine), are based on energy functions. Through energy clustering with the given threshold setting, objects with different semantic information are assigned to various categories [28].After segmenting the plane candidate setPπ1,π2,⋯,πN of the structured scene, it is essential to effectively filter those planes affecting the accuracy of the point cloud cluster in the global map. Here, a point cloud segmentation optimization algorithm based on the energy function according to the Manhattan hypothesis is given. Considering that there are many regular plane geometric constraints in structured scenes, such as corridors, walls, and desks indoors, and that most of the plane structures are perpendicular or parallel to each other, an energy function can be naturally defined by these constraints and could be used to filter the final point cloud segmentation result C=c1,c2,⋯,cM by minimizing the energy function.The energy function of point cloud semantic segmentation mainly includes first-order data items and second-order spatial constraint items, where the data item represents the fitting degree of the point cloud in the plane setP, and the spatial constraint item represents the geometric constraint relationship (perpendicular or parallel) between the planes in the structured scene. Then, the energy function is written as follows:
(7)EP=∑πn∈PE1πn+∑πm,πn∈PE2πm,πn,C=argminEP,where EP represents the total energy function, C is the optimized final point cloud cluster set, E1 is the first-order data item, and E2 represents the spatial constraint-related item.ForE1, the candidate set Pπ1,π2,⋯,πN is determined using the distance between the plane’s centroid and the structured scene point cloud classified to the plane. The fitting degree between the plane candidate set and the point cloud can be determined; therefore, those candidates with a low degree can be deleted. The point cloud set corresponding to the candidate plane in the scene can be expressed as follows:
(8)Cm=pidπpG,pi<σ1,where dπpG,pi is the distance from point cloud pi to centroid πpG of a certain candidate plane, Np is the point cloud set corresponding to the candidate plane in the scene, and σ1 is the distance threshold (σ1 is set to 20 cm). The more point clouds with distance from the candidate plane within the threshold are classified as planes; the more attribution point clouds better represent the plane fit. Then, the data item E1 can be described as follows:
(9)E1πn=1−Npλ,Np<λ,expλ−Npλ,Np≥λ,where λ is the threshold of the degree of fit between the plane candidate set and the point cloud, and it is set as 40 in our test.Since there are many mutually perpendicular or parallel planes in the structured scene, if they can be transferred to an item in the energy function, the optimization result will be close to the selection of clustering point clouds into a set of candidate planes that are parallel or perpendicular to each other. The geometric structure constraint-related itemE2 is defined as follows:
(10)E2πm,πn=1−exp−φ′πm,πnσ2,φ′πm,πn=minφπm,πn,π2−φπm,πn,where φπm,πn is the angle between planes πm and πn in the set of candidate planes, φ′πm,πn is the angle difference between the two planes that are parallel or perpendicular to each other, and σ2 represents the preset increase step of the angle (σ2 is set 10°). When the two planes are sufficiently parallel or perpendicular to each other (parallel is set to 10° and perpendicular is set to 80° in the experiment), the value of φ′πm,πn is 0, and the added energy term E2πm,πn is also 0. Moreover, the three-dimensional energy of the energy function will not increase, and the plane may exist in the final plane segmentation set. If the correlation between the geometric constraints of the two planes is lower, neither perpendicular nor parallel, then the energy value will be larger, and the algorithm will tend to eliminate the plane.Given the candidate setPπ1,π2,⋯,πN, each plane is only selected or eliminated by one category, and thus, the result of the energy function is as follows:
(11)EP=∑n=1NE1Inbn+∑pm,pn∈PE2Im,Inbmbn,where B=b1,b2,⋯,bN is a binary set to represent whether the plane in the candidate set Pπ1,π2,⋯,πN is selected, bn=1 means πn is selected for the final set, E1 represents the first-order data item, and E2 represents the geometric structure constraint-related items. Therefore, the simplified form of the energy function can be written as follows:
(12)EB=BTU+BTMB,B∈0,1,where U is the first-order data item of the energy function and M=mij∈R represents the correlation coefficient of two candidate planes, which is a symmetric matrix. When two planes are not correlated, then mij=0.This paper introduces an iterative trust-region optimization algorithm to solve the energy function. In the process, an approximate model of the optimization problem is constructed in the trust region of the current resultBt; then, the final set is obtained by optimizing the global model in the trust region. Meanwhile, the trust region needs to be based on the current approximation during the iteration. Thus, the above equation is expressed as the sum of a positive correlation term and a negative correlation term as follows:
(13)EB=ENB+EPB,ENB=BTU+BTM−B,EPB=BTM+B,where M− and M+ represent the part of the correlation coefficient mij−≤0 and mij+≥0 in the symmetric matrix 𝑀, respectively. Given the current result Bt, the energy function EB can be approximated in the trust region as follows:
(14)EtB=ENB+BTUt+c,where Ut=2M+Bt and Ut+c are first-order Taylor expansions from EPB, and we can solve the energy function EtB in the trust region σt as follows:
(15)B∗=argminB−Bt<σtEtB.The above energy function is transformed by solving the subproblem of unconstrained Lagrangian optimization in the trust region. The subproblem equation of the transformed trust region is as follows:(16)LtB=EtB+λt·B−Bt,where λt decides the size of the trust region. The algorithm updates λt through each iteration adaptively according to the inversely proportional relationship between λt and σt, consequently updating the size of the trust region. After obtaining the candidate result B∗, the quality of the candidate result can be evaluated according to the ratio between the actual energy reduction R and the predicted energy reduction P of the energy function. The quality of the approximate function is evaluated by calculating R/P. If R/P exceeds the given threshold σ1, the optimal result is updated and the range of the trust region λt is adjusted as follows:
(17)λt+1⟵λtα,RP>σ2,λt·α,RP≤σ2.To filter the candidate results, the threshold parameterλt is set to 1, σ2 is set to 0.25, and α is set to 2. When the decreased rate of the energy function exceeds the threshold, the algorithm expands the range of the trust region λt. By iteratively solving the subproblems of unconstrained Lagrangian optimization under the trust region, the final filtered set C=c1,c2,⋯,cM is obtained.Similar to the process of constructing the energy function of planes,Ql1,l2,⋯,lN is defined as the edge candidate set and F=f1,f2,⋯,fM as the final point cloud segmentation result. According to the Manhattan hypothesis, the energy function for the edges can be defined as follows:
(18)EQ=∑ln∈QE1ln+∑lm,ln∈QE2lm,ln,F=argminEQ.ForE1, by calculating the distance between the geometric centre of the candidate edge and the structured scene point cloud classified to the edge, the degree of the fitting is obtained to filter the set Ql1,l2,⋯,lN. The point cloud set of candidate edges is as follows:
(19)Fm=pidlpG,pi<ε1,where dlpG,pi is the distance from point cloud pi to geometric centre lpG of a certain candidate edge, Np is the point cloud set corresponding to the candidate edge in the scene, and ε1 is the distance threshold. The data item E1 is expressed as follows:
(20)E1ln=1−Npλ,Np<λ,expλ−Npλ,Np≥λ,where λ is the threshold of the degree of fit between the edge candidate set and the point cloud and it is set as 10 in our test.Since the contour vectors of plane features are mostly parallel or perpendicular to each other, the geometric structure constraint itemE2 is added to the energy function in the candidate edge filtering process. Similar to plane geometric structure constraints, the geometric structure constraint related to item E2 is expressed as follows:
(21)E2lm,ln=1−exp−φ′lm,lnε2,φ′lm,ln=minφlm,ln,π2−φlm,ln,where φlm,ln is the vector angle between lm and ln in the set of candidate edges, φ′lm,ln is the two candidate edges that should match the angle between the parallel or perpendicular set constraints, and ε2 is the set angle increase step length.Similar to the process of solving the energy function of planes, the inappropriate edges are eliminated, and the final candidate set is obtained.


## 3.4. Back-End Graph Optimization Using a Point-Edge-Plane-Based Pose Graph

Factor graphs are frequently used in SLAM systems to express the back-end optimization problem since they are more suitable for performing reasoning than Bayesian networks. Under the assumption of the Gaussian noise model, this problem is equivalent to solving the nonlinear least-squares problem.


### 3.4.1. Graph Optimization Structure Overview

In this paper, a novel factor graph for back-end bundle adjustment is designed to include points, edges, planes, and their spatial constraints to improve the stability and positioning accuracy of the system. Figure2 includes three factors: (a) point landmarks, (b) edge landmarks, and (c) plane landmarks. Based on the Manhattan hypothesis, there are structural relationship constraints between the plane landmark and the edge landmark. Therefore, there are 5 residual values in the graph optimization structure of this article: (a) point landmark reprojection error, (b) plane landmark reprojection error, (c) edge landmark reprojection error, (d) plane landmark structural error, and (e) edge landmark structural error.Figure 2
A novel factor graph for deeply exploring the spatial constraints based on the Manhattan hypothesis.


### 3.4.2. Error Items of Point Landmark Factors

The error item of a point landmark can be calculated by reprojecting it into pixel coordinates as the observation point coordinate to obtain the reprojection error, which can be written as follows:(22)eppw,Tcw=z−ρTcwpw,where ep is the reprojection error, ρp is the pixel coordinates of the point landmark, and z≜us,vsT are the pixel coordinates of the observation point. We let zij be the pixel coordinates of landmark pwj observed at pose Tcwi, and the overall error items of all observed point landmarks during a certain period can be described by the equation as follows:
(23)Ep=12∑i=1m∑j=1nepTcwjpwj2=12∑i=1m∑j=1nzij−ρTcwipwj2.


### 3.4.3. Error Items of Plane Landmark Factors

Generally, there are six degrees of freedom for a rigid object in three-dimensional space: translations in thex, y, and s directions and rotations around the x‐y‐s axis. However, when the rigid body’s motion is limited to the plane, only two degrees of freedom of the translational motion and the rotation axis are fixed. Therefore, this paper adopts the minimum parametrized plane expression τ=φ,ψ,d to avoid the hyperparameter problem as follows:
(24)τ=qπ=φ=tan−1nynx,ψ=sin−1nz,dT,where φ∈−π,π and ψ∈−π,π are the azimuth and elevation angles of the normal, respectively. The error item of a single observation plane can be written as follows:
(25)eπ1πw,Tcw=qπ−qTcw−Tπw,where qπ is the minimized parameter coordinate of the observation plane. After considering the observations at other moments, we let qπij be the parameter coordinates of plane πwj observed at pose Tcwi. The error item of the observation part can be written as follows:
(26)Eπ1=12∑i=1m∑j=1neππwj,Tcwi2=12∑i=1m∑j=1nqπij−qTcwi−Tπwj2.In addition, according to the Manhattan hypothesis, there are parallel and perpendicular structural relationships between plane landmarks; therefore, the error item for plane landmark factors is given as follows:(27)eπ2πw,Tcw=mineπp,eπv=minqnn−qnRcwnw,qnRtgn−qnRcwnw,where eπp is the parallel constraint error, eπv is the perpendicular constraint error, qn refers to the azimuth and elevation angle, n is the observation plane normal, Rcw is the rotation matrix part of the pose transformation matrix Tcw, and Rtg is an additional rotation matrix to rotate the normal to the same direction. Considering the observations at other moments, we let qnnij be the normal of plane πwj observed at pose Tcwi, and the error item of the structural constraint part can be written as follows:
(28)Eπ2=12∑i=1m∑j=1nminqnnij−qnRcwinwj,qnRtgnij−qnRcwinwj2.


### 3.4.4. Error Items of Edge Landmark Factors

A straight line in 3-D space has only four degrees of freedom since nothing is changed when a straight line rotates around itself or translates along its own direction. In this paper, the minimum parametrized edge expressionε=φ,ψ,ulTT is used to avoid the hyperparameter problem as follows:
(29)ε=sπ=φ=tan−1nynx,φ=sin−1nz,ρplTT.The error item for a single observation edge can be defined as follows:(30)el1lw,Tcw=sl−sTcw−Tlw,where sl is the minimized parameter coordinate of the edge of the current observation. Additionally, we let slij be the parameter coordinate of edge lwj observed at pose Tcwi; then, the error item of the observation edge landmarks during a certain time can be described as follows:
(31)El1=12∑i=1m∑j=1nellwj,Tcwi2=12∑i=1m∑j=1nslij−sTcwi−Tlwj2.Since there are parallel or perpendicular structural relationships among edge landmarks, the error item considering such constraints can be written as follows:(32)el2πw,Tcw=minelp,elv=minsnn−snRcwnw,snRtgn−snRcwnw,where elp is the parallel constraint error, elv is the perpendicular constraint error, and n is the observed edge direction vector. Considering the observations during certain times, snnij is assumed to be the direction vector of edge lwj observed at pose Tcwi, and the error item of the structural constraint part can be described as follows:
(33)El2=12∑i=1m∑j=1nminsnnij−snRcwinwj,snRtgnij−snRcwinwj2.


## 3.4.1. Graph Optimization Structure Overview

In this paper, a novel factor graph for back-end bundle adjustment is designed to include points, edges, planes, and their spatial constraints to improve the stability and positioning accuracy of the system. Figure2 includes three factors: (a) point landmarks, (b) edge landmarks, and (c) plane landmarks. Based on the Manhattan hypothesis, there are structural relationship constraints between the plane landmark and the edge landmark. Therefore, there are 5 residual values in the graph optimization structure of this article: (a) point landmark reprojection error, (b) plane landmark reprojection error, (c) edge landmark reprojection error, (d) plane landmark structural error, and (e) edge landmark structural error.Figure 2
A novel factor graph for deeply exploring the spatial constraints based on the Manhattan hypothesis.


## 3.4.2. Error Items of Point Landmark Factors

The error item of a point landmark can be calculated by reprojecting it into pixel coordinates as the observation point coordinate to obtain the reprojection error, which can be written as follows:(22)eppw,Tcw=z−ρTcwpw,where ep is the reprojection error, ρp is the pixel coordinates of the point landmark, and z≜us,vsT are the pixel coordinates of the observation point. We let zij be the pixel coordinates of landmark pwj observed at pose Tcwi, and the overall error items of all observed point landmarks during a certain period can be described by the equation as follows:
(23)Ep=12∑i=1m∑j=1nepTcwjpwj2=12∑i=1m∑j=1nzij−ρTcwipwj2.


## 3.4.3. Error Items of Plane Landmark Factors

Generally, there are six degrees of freedom for a rigid object in three-dimensional space: translations in thex, y, and s directions and rotations around the x‐y‐s axis. However, when the rigid body’s motion is limited to the plane, only two degrees of freedom of the translational motion and the rotation axis are fixed. Therefore, this paper adopts the minimum parametrized plane expression τ=φ,ψ,d to avoid the hyperparameter problem as follows:
(24)τ=qπ=φ=tan−1nynx,ψ=sin−1nz,dT,where φ∈−π,π and ψ∈−π,π are the azimuth and elevation angles of the normal, respectively. The error item of a single observation plane can be written as follows:
(25)eπ1πw,Tcw=qπ−qTcw−Tπw,where qπ is the minimized parameter coordinate of the observation plane. After considering the observations at other moments, we let qπij be the parameter coordinates of plane πwj observed at pose Tcwi. The error item of the observation part can be written as follows:
(26)Eπ1=12∑i=1m∑j=1neππwj,Tcwi2=12∑i=1m∑j=1nqπij−qTcwi−Tπwj2.In addition, according to the Manhattan hypothesis, there are parallel and perpendicular structural relationships between plane landmarks; therefore, the error item for plane landmark factors is given as follows:(27)eπ2πw,Tcw=mineπp,eπv=minqnn−qnRcwnw,qnRtgn−qnRcwnw,where eπp is the parallel constraint error, eπv is the perpendicular constraint error, qn refers to the azimuth and elevation angle, n is the observation plane normal, Rcw is the rotation matrix part of the pose transformation matrix Tcw, and Rtg is an additional rotation matrix to rotate the normal to the same direction. Considering the observations at other moments, we let qnnij be the normal of plane πwj observed at pose Tcwi, and the error item of the structural constraint part can be written as follows:
(28)Eπ2=12∑i=1m∑j=1nminqnnij−qnRcwinwj,qnRtgnij−qnRcwinwj2.


## 3.4.4. Error Items of Edge Landmark Factors

A straight line in 3-D space has only four degrees of freedom since nothing is changed when a straight line rotates around itself or translates along its own direction. In this paper, the minimum parametrized edge expressionε=φ,ψ,ulTT is used to avoid the hyperparameter problem as follows:
(29)ε=sπ=φ=tan−1nynx,φ=sin−1nz,ρplTT.The error item for a single observation edge can be defined as follows:(30)el1lw,Tcw=sl−sTcw−Tlw,where sl is the minimized parameter coordinate of the edge of the current observation. Additionally, we let slij be the parameter coordinate of edge lwj observed at pose Tcwi; then, the error item of the observation edge landmarks during a certain time can be described as follows:
(31)El1=12∑i=1m∑j=1nellwj,Tcwi2=12∑i=1m∑j=1nslij−sTcwi−Tlwj2.Since there are parallel or perpendicular structural relationships among edge landmarks, the error item considering such constraints can be written as follows:(32)el2πw,Tcw=minelp,elv=minsnn−snRcwnw,snRtgn−snRcwnw,where elp is the parallel constraint error, elv is the perpendicular constraint error, and n is the observed edge direction vector. Considering the observations during certain times, snnij is assumed to be the direction vector of edge lwj observed at pose Tcwi, and the error item of the structural constraint part can be described as follows:
(33)El2=12∑i=1m∑j=1nminsnnij−snRcwinwj,snRtgnij−snRcwinwj2.


## 3.5. Pose Optimization

In Section3.4, several error items are introduced for back-end graph optimization based on the Manhattan hypothesis. In the tracking process, the system will optimize using point, edge, and plane features and the relationships among them using a sliding window through local BA and detect loop closure. When a loop closure is detected, the system will terminate local optimization and perform global optimization.By using the aforementioned error terms, the camera pose in the local map is optimized by local BA by minimizing the reprojection error of those point, edge, and plane features seen in these keyframes, as well as the structural error of those planes and the edges in the sliding window. The features observed by all keyframes outside the sliding window also contribute to the error item, but they are kept fixed in the optimization. Finally, the optimization problem can be transformed into the optimization problem as follows:(34)Tcw=argminHpEp+Hπ1Eπ1+Hπ2Eπ2+Hl1El1+Hl2El2,where Hx is the Huber robust cost function and E is the error term introduced above. To resolve the aforementioned optimization problem, the Levenberg-Marquardt method and g2o [29] can be used.


## 4. Experiments

### 4.1. Point Cloud Map Construction Experiment

The performance of our feature filtering algorithm was compared with SP-SLAM, a mainstream SLAM system that supports extracting structural features. To ensure the system is not affected by noise features and cumulative errors, we choose the indoor small-scale multiobject scene, indoor large-scale scene, and indoor structured scene. In our test design,fr1 in the experiment represents the indoor rich texture scene, which is used to test the degree of the system affected by noise features; fr2-large represents indoor large-scale scenes, which are used to judge the ability to eliminate the cumulative errors; and fr3 represent indoor small-scale structured scenes, and they are used to test the performance of the system in low-texture scenes. Experiments are carried out on the TUM dataset, and the reconstructed point-cloud results are shown in Figure 3.Figure 3
Supposed planes generated in the tracking process. Density of the point cloud using different colors, sorted as gray, black, blue, and red, and we set a plane with gray color if its point number is between [25000, +∞], black for [12000, 25000], blue for [6000, 12000], and red for [0, 6000], respectively.From the test results, it can be found that the point cloud map construction result in this paper is more complete than SP-SLAM in the point cloud segmentation of the floor plane since we have more gray planes and less red planes, and the connectivity status between the planes is better.The datasets fr1_desk and fr1_room are indoor small-scale multiobject datasets. From Figures3(a) and 3(b), it was found that the point cloud map construction result in this paper is more complete than SP-SLAM in the point cloud segmentation of the floor plane, and the connectivity status between the planes is better. The segmentation of the storage cabinet, the desktop, and the main objects on the desktop, such as computers and keyboards, is more accurate. However, it is difficult to segment the potted structure, and smaller objects, such as mouse and water glasses, blend in with the tabletop because the point cloud noise of the objects is too large. Meanwhile, with the growth of the initial plane, an increasing number of small plane areas are eliminated to accelerate the processing speed. As a comparative test, SP-SLAM mismatches the same wall and the locker moving in the image sequence; therefore, it generates a mismatched plane.As shown in Figure3(c), the dataset used is fr2_large_with_loop, which is in a large-scale indoor environment. From the green wireframe, it can be seen that the large plane has partly complemented in the large-scale scene. For the result of point cloud segmentation, the brown plane represents the ground, and other colors represent different objects, such as walls and furniture. Compared with SP-SLAM, the consistency of the walls in the point cloud map construction results of this paper’s method is enhanced due to the partial elimination of mismatch, and the segmentation of the wall and the ground point clouds is more complete.fr3_structure_notex_near is captured from a structured scene. As shown in Figure3(d), our method accurately delineates the boundaries of different walls, while the different walls of SP-SLAM intrude and intersect each other, thus making the segmentation of the point cloud erroneous. Therefore, the point cloud map results constructed by our method are more accurate than those of SP-SLAM in segmenting the point clouds of different walls.The results of the quantitative comparison of feature filtering are shown in Table1, where N1 represents the number of plane landmarks and N2 represents the average number of supposed planes for every frame. As we can see, the average number of feature extractions per frame in the fr1 and fr2 series of the dataset by this algorithm is reduced by 17.3% compared with the SP-SLAM system, and the total number of extracted features is reduced by 24.6%, which is because these datasets contain many planes with high noise. Our approach eliminates noisier features using the energy function introduced in Section 3.3 and merges smaller objects into larger planes. We also find that the average number of supposed planes of each frame in the fr3 series and the number of plane extractions are increased by 8.16% compared with the SP-SLAM system, which indicates that the algorithm is more suitable for feature detection and point cloud segmentation in low-texture structured scenarios.Table 1
Plane extraction results on the TUM dataset.
SP-SLAMOur methodPlane featureN1Plane numberN2Plane featureN1Plane numberN2fr1_desk181.529141.284fr1_desk2191.719121.142fr1_floor111.43381.212fr1_room442.108331.541fr2_large_no_loop141.097121.018fr2_large_with_loop180.791140.751fr3_notexture_far52.95253.147fr3_notexture_near51.68251.919fr3_texture_far73.12163.239


### 4.2. System Accuracy Evaluation

To evaluate the performance of our approach, experiments are carried out with several multifeature SLAM frameworks: ORB-SLAM2 as the most widely used traditional SLAM method; PL-SLAM supporting both points and line segment features and using them in map construction and BA; Planar-SLAM using points, lines, and planes into a decoupling-refinement method and utilizing Manhattan relationships for system optimization; PLP-SLAM utilizing points and line segments for robust camera localization while planes are used to intuitively illustrate the structure of the scene; and SP-SLAM using points, lines, and planes to constraint features while Manhattan relationships being combined with multifeature to optimization. The RMSE is used to evaluate the accuracy performance. As shown in Table2, compared with SP-SLAM, the proposed method has an improvement rate of 18.07% in output trajectory accuracy. Figure 4 shows the comparison of the RMSE on the TUM RGB-D dataset. It was found that fully using structural features and the constraints among them improves the accuracy and robustness of the SLAM system.Table 2
An RMSE comparison of the absolute trajectory error in the TUM RGB-D test.
SequenceORB-SLAM2PL-SLAMPlanar-SLAMPLP-SLAMSP-SLAMOur methodfr1_desk0.01660.01370.2180.02020.01250.0105fr1_desk20.01760.01850.23—0.01680.0147fr1_floor0.03170.0759×0.01210.02550.0215fr1_room0.05770.05210.0657—0.05090.0432fr2_no_loop0.15270.18070.5905—0.21580.1325fr2_with_loop0.11460.15270.6583—0.19780.1632fr3_notex_far0.03580.02940.0160.03520.02320.0179fr3_notex_near0.02720.02160.02070.00160.01840.0153fr3_tex_far0.01660.01250.02210.00890.009950.00920× indicates that the algorithm fails due to loss. — means result is not available.Figure 4
Comparison of the RMSE on the TUM RGB-D dataset.Furthermore, the comparison of the estimated trajectories of different algorithms is shown in Figure5. The dotted line is the ground-truth trajectory, the blue line is the ORB-SLAM2 trajectory, the green line is the SP-SLAM trajectory, and the red line is the output trajectory of our method. Because the ground-truth trajectory is more complicated, the optimization effect of the algorithm in this paper can be seen in the local trajectory. Figure 6 shows the positioning errors of different algorithms in the X and Y directions, where we can see that the errors of our algorithm are less than those of the existing algorithms.Figure 5
Comparison of the RMSE on the TUM RGB-D dataset.Figure 6
Comparison of the RMSE on the TUM RGB-D dataset.
(a)
fr3_structure_notexture_near(b)
fr2_large_no_loop


### 4.3. Runtime Analysis

Tables3 and 4 show the runtime analysis of the systems running on the TUM dataset. All the experiments run on a desktop computer with i7-8700HQ 3.20 GHz CPU, 32.0 GB RAM, without GPU, and the operating system is Ubuntu 16.04.Table 3
Average runtime (unit: ms) of different components.
RuntimePlane segmentationEdge extractionsLocal BAGlobal BATracking12.73.1128.3316.544.9Table 4
Average runtime (unit: ms) of fr1_room sequence.
RuntimeTrackingLocal mappingLocal BAOur method20.1817.452134.24SP-SLAM17.9120.369154.77For the back end, our proposal is a little bit faster than the original one (SP-SLAM), and the root cause is that the energy function offsets the computational loss of edge feature extraction and optimization. For the front end, nothing is modified for keyframe deduction, and the tracking time consumption is only 44.9 ms, so it is fast enough for real-time tracking. We also measured local BA time consumption which is 128.3 ms, since it is executed in a dedicated thread, so it is also fast enough for real-time computing.


## 4.1. Point Cloud Map Construction Experiment

The performance of our feature filtering algorithm was compared with SP-SLAM, a mainstream SLAM system that supports extracting structural features. To ensure the system is not affected by noise features and cumulative errors, we choose the indoor small-scale multiobject scene, indoor large-scale scene, and indoor structured scene. In our test design,fr1 in the experiment represents the indoor rich texture scene, which is used to test the degree of the system affected by noise features; fr2-large represents indoor large-scale scenes, which are used to judge the ability to eliminate the cumulative errors; and fr3 represent indoor small-scale structured scenes, and they are used to test the performance of the system in low-texture scenes. Experiments are carried out on the TUM dataset, and the reconstructed point-cloud results are shown in Figure 3.Figure 3
Supposed planes generated in the tracking process. Density of the point cloud using different colors, sorted as gray, black, blue, and red, and we set a plane with gray color if its point number is between [25000, +∞], black for [12000, 25000], blue for [6000, 12000], and red for [0, 6000], respectively.From the test results, it can be found that the point cloud map construction result in this paper is more complete than SP-SLAM in the point cloud segmentation of the floor plane since we have more gray planes and less red planes, and the connectivity status between the planes is better.The datasets fr1_desk and fr1_room are indoor small-scale multiobject datasets. From Figures3(a) and 3(b), it was found that the point cloud map construction result in this paper is more complete than SP-SLAM in the point cloud segmentation of the floor plane, and the connectivity status between the planes is better. The segmentation of the storage cabinet, the desktop, and the main objects on the desktop, such as computers and keyboards, is more accurate. However, it is difficult to segment the potted structure, and smaller objects, such as mouse and water glasses, blend in with the tabletop because the point cloud noise of the objects is too large. Meanwhile, with the growth of the initial plane, an increasing number of small plane areas are eliminated to accelerate the processing speed. As a comparative test, SP-SLAM mismatches the same wall and the locker moving in the image sequence; therefore, it generates a mismatched plane.As shown in Figure3(c), the dataset used is fr2_large_with_loop, which is in a large-scale indoor environment. From the green wireframe, it can be seen that the large plane has partly complemented in the large-scale scene. For the result of point cloud segmentation, the brown plane represents the ground, and other colors represent different objects, such as walls and furniture. Compared with SP-SLAM, the consistency of the walls in the point cloud map construction results of this paper’s method is enhanced due to the partial elimination of mismatch, and the segmentation of the wall and the ground point clouds is more complete.fr3_structure_notex_near is captured from a structured scene. As shown in Figure3(d), our method accurately delineates the boundaries of different walls, while the different walls of SP-SLAM intrude and intersect each other, thus making the segmentation of the point cloud erroneous. Therefore, the point cloud map results constructed by our method are more accurate than those of SP-SLAM in segmenting the point clouds of different walls.The results of the quantitative comparison of feature filtering are shown in Table1, where N1 represents the number of plane landmarks and N2 represents the average number of supposed planes for every frame. As we can see, the average number of feature extractions per frame in the fr1 and fr2 series of the dataset by this algorithm is reduced by 17.3% compared with the SP-SLAM system, and the total number of extracted features is reduced by 24.6%, which is because these datasets contain many planes with high noise. Our approach eliminates noisier features using the energy function introduced in Section 3.3 and merges smaller objects into larger planes. We also find that the average number of supposed planes of each frame in the fr3 series and the number of plane extractions are increased by 8.16% compared with the SP-SLAM system, which indicates that the algorithm is more suitable for feature detection and point cloud segmentation in low-texture structured scenarios.Table 1
Plane extraction results on the TUM dataset.
SP-SLAMOur methodPlane featureN1Plane numberN2Plane featureN1Plane numberN2fr1_desk181.529141.284fr1_desk2191.719121.142fr1_floor111.43381.212fr1_room442.108331.541fr2_large_no_loop141.097121.018fr2_large_with_loop180.791140.751fr3_notexture_far52.95253.147fr3_notexture_near51.68251.919fr3_texture_far73.12163.239


## 4.2. System Accuracy Evaluation

To evaluate the performance of our approach, experiments are carried out with several multifeature SLAM frameworks: ORB-SLAM2 as the most widely used traditional SLAM method; PL-SLAM supporting both points and line segment features and using them in map construction and BA; Planar-SLAM using points, lines, and planes into a decoupling-refinement method and utilizing Manhattan relationships for system optimization; PLP-SLAM utilizing points and line segments for robust camera localization while planes are used to intuitively illustrate the structure of the scene; and SP-SLAM using points, lines, and planes to constraint features while Manhattan relationships being combined with multifeature to optimization. The RMSE is used to evaluate the accuracy performance. As shown in Table2, compared with SP-SLAM, the proposed method has an improvement rate of 18.07% in output trajectory accuracy. Figure 4 shows the comparison of the RMSE on the TUM RGB-D dataset. It was found that fully using structural features and the constraints among them improves the accuracy and robustness of the SLAM system.Table 2
An RMSE comparison of the absolute trajectory error in the TUM RGB-D test.
SequenceORB-SLAM2PL-SLAMPlanar-SLAMPLP-SLAMSP-SLAMOur methodfr1_desk0.01660.01370.2180.02020.01250.0105fr1_desk20.01760.01850.23—0.01680.0147fr1_floor0.03170.0759×0.01210.02550.0215fr1_room0.05770.05210.0657—0.05090.0432fr2_no_loop0.15270.18070.5905—0.21580.1325fr2_with_loop0.11460.15270.6583—0.19780.1632fr3_notex_far0.03580.02940.0160.03520.02320.0179fr3_notex_near0.02720.02160.02070.00160.01840.0153fr3_tex_far0.01660.01250.02210.00890.009950.00920× indicates that the algorithm fails due to loss. — means result is not available.Figure 4
Comparison of the RMSE on the TUM RGB-D dataset.Furthermore, the comparison of the estimated trajectories of different algorithms is shown in Figure5. The dotted line is the ground-truth trajectory, the blue line is the ORB-SLAM2 trajectory, the green line is the SP-SLAM trajectory, and the red line is the output trajectory of our method. Because the ground-truth trajectory is more complicated, the optimization effect of the algorithm in this paper can be seen in the local trajectory. Figure 6 shows the positioning errors of different algorithms in the X and Y directions, where we can see that the errors of our algorithm are less than those of the existing algorithms.Figure 5
Comparison of the RMSE on the TUM RGB-D dataset.Figure 6
Comparison of the RMSE on the TUM RGB-D dataset.
(a)
fr3_structure_notexture_near(b)
fr2_large_no_loop


## 4.3. Runtime Analysis

Tables3 and 4 show the runtime analysis of the systems running on the TUM dataset. All the experiments run on a desktop computer with i7-8700HQ 3.20 GHz CPU, 32.0 GB RAM, without GPU, and the operating system is Ubuntu 16.04.Table 3
Average runtime (unit: ms) of different components.
RuntimePlane segmentationEdge extractionsLocal BAGlobal BATracking12.73.1128.3316.544.9Table 4
Average runtime (unit: ms) of fr1_room sequence.
RuntimeTrackingLocal mappingLocal BAOur method20.1817.452134.24SP-SLAM17.9120.369154.77For the back end, our proposal is a little bit faster than the original one (SP-SLAM), and the root cause is that the energy function offsets the computational loss of edge feature extraction and optimization. For the front end, nothing is modified for keyframe deduction, and the tracking time consumption is only 44.9 ms, so it is fast enough for real-time tracking. We also measured local BA time consumption which is 128.3 ms, since it is executed in a dedicated thread, so it is also fast enough for real-time computing.


## 5. Conclusions

Providing mapping and localization functionality continuously and robustly in structured scenes with low texture is a great challenge for visual SLAM systems. This paper proposes a novel visual SLAM method based on the Manhattan hypothesis. The method filters the structural features through an energy function in the front end and adds the constraints of the Manhattan hypothesis relationships to the back-end optimization, which improves the precision and robustness of the system. The test results show that the proposed method provides a better positioning ability, with the positioning accuracy improved by 18.07% on average on the TUM dataset compared with the existing algorithms.Future research will focus on tightly integrating IMU and digital map information to further improve the synchronization positioning and mapping accuracy in structured scenes with sparse feature points and low texture.



---

*Source: 4152171-2023-01-31.xml*