An indoor visual SLAM method based on adaptive fusion of point-line-surface features

Abstract

The application provides a VSLAM method based on point-line-surface feature adaptive fusion, a method for positioning and mapping by adaptively selecting a feature fusion strategy based on the extraction and matching results of point-line-surface features in an image, that is, selecting a suitable method according to the richness of texture, thereby solving the problem that the traditional visual SLAM cannot be used in a non-texture or weak-texture environment, and improving the operation efficiency. The VSLAM provided by the application can adaptively select a point-line-surface feature fusion strategy according to the richness of texture in an indoor environment, thereby solving the problems of tracking failure and positioning failure, and improving the operation efficiency of the VSLAM system.

Description

Technical Field

[0001] This invention belongs to the field of indoor positioning and navigation technology, specifically relating to an indoor visual SLAM method based on adaptive fusion of point, line, and surface features. Background Technology

[0002] In recent years, with the widespread application of robots, Simultaneous Localization and Mapping (SLAM) technology has become a key technology for robots to achieve autonomous navigation in environments lacking prior map knowledge. SLAM refers to a subject equipped with specific sensors building an environmental model through motion and estimating its own motion simultaneously, without prior environmental knowledge. Mainstream SLAM can be divided into two types based on the sensors used for data acquisition: LiDAR-based mapping and navigation, and Visual SLAM (VSLAM) based on monocular / binocular / RGB-D cameras. Early SLAM technologies used LiDAR to collect angle and distance information to complete the robot's localization. While LiDAR is accurate in ranging, has a simple model, and mature theoretical research, it is expensive and has limited map information, making it unsuitable for use in textured indoor scenes. Compared to LiDAR, cameras have advantages such as simple structure, low cost, strong scene recognition capabilities, and rich perception of texture information. Therefore, camera-based VSLAM has become a current research focus.

[0003] Traditional VSLAM primarily relies on feature point extraction for tracking and mapping. Point features, due to their mature extraction and matching algorithms, have become the mainstream VSLAM approach. Indoor environments are complex environments with uneven texture richness, making traditional VSLAM unsuitable for scenes with limited texture. Traditional point-feature-based VSLAM depends on image information to recover pose. If texture information in the environment is scarce or too weak, image information becomes inadequate, and point-feature-based methods fail to extract enough feature points, leading to tracking failure. Indoor scenes are predominantly artificially structured environments, and abundant line and planar features, as higher-dimensional features, are excellent complements to the lack of point features in weakly textured environments. Therefore, researchers have increasingly focused on the relatively abundant line and planar features in indoor environments. Most indoor weakly textured environments, such as corridors, lobbies, and parking lots, exhibit rich structural information; walls, for example, present planes that can be used to calculate the relative direction and distance between walls and the camera. Planar features are rich in semantic information and have a compact mapping structure, reducing information redundancy. Numerous VSLAM studies based on planar features have been applied to indoor localization. Current research primarily utilizes point-line fusion and point-line-area fusion methods for indoor scene localization and mapping. These methods aim to better characterize the structural features of the scene and improve VSLAM performance. While these fusion methods can address the tracking failure issue of VSLAM in textureless or weakly textured indoor environments, indoor scenes are rarely entirely textureless; many still contain scenes with abundant texture. Continuously employing multiple feature fusion methods would significantly increase the computational load and runtime of the system. Therefore, for indoor environments with uneven texture richness, adaptively selecting feature fusion strategies for VSLAM remains a worthy area of ​​research. Summary of the Invention

[0004] The purpose of this invention is to provide an indoor visual SLAM method based on adaptive fusion of point, line and surface features.

[0005] The objective of this invention is achieved through the following technical solution:

[0006] An indoor visual SLAM method based on adaptive fusion of point, line, and surface features, with the following specific steps:

[0007] Step 1: Feature extraction and matching;

[0008] Extract ORB feature points from the image, describe the feature points using BRIEF descriptors, and achieve point feature matching between adjacent image frames;

[0009] Line detection in the image is performed using the LSD method, and then line feature matching is performed using line segment descriptors;

[0010] Surface features are extracted using hierarchical clustering.

[0011] Step 2: Mode selection;

[0012] After obtaining the matching relationship between point, line, and surface features between adjacent frames, the mode selection begins based on the matching relationship between point, line, and surface features.

[0013] Step 3: Pose estimation;

[0014] After extracting the point, line, and surface features required by the system, pose estimation begins.

[0015] Furthermore, in step 1, surface features are extracted using a hierarchical clustering method;

[0016] First, the point cloud in the image is uniformly distributed into N nodes, and nodes with insufficient or discontinuous depth information are removed. Then, the feature values ​​of each pixel in the image are clustered, and continuous blocks with feature differences within a threshold range are clustered into the same segmentation block. Finally, the output plane parameters of each pixel are iteratively optimized to extract the planar features from the image. After extracting the planar features, we calculate the angle between the normal vectors of two planes and the distance between the two planes. When the angle between the normal vectors is less than θ and the distance between the two planes is less than d, the two planes are considered to be matched.

[0017] Furthermore, step 2 specifically includes:

[0018] First, we divide our system into four modes: point, point-line, point-plane, and point-line-plane, and run it in multiple scenarios. We obtain the minimum, maximum, and average number of point, line, and plane feature matching for each scenario. We also obtain the root mean square error (RMSE) of the trajectory of the four modes in different scenarios, calculated as follows, as well as the total system running time tt and the average tracking time per frame tm.

[0019]

[0020] in, The keyframe trajectory represented by x is estimated by the system. i This represents the actual camera pose;

[0021] After obtaining the required data, we determine which of the four fusion methods is best in a given scenario by setting an evaluation criterion value P. The evaluation value P is calculated as follows:

[0022]

[0023] The total running time of the system, the average running time per frame, and the root mean square error of the running trajectory are selected for evaluation, and the weights are assigned as α, β, and γ, respectively.

[0024] By comparing the value of P, the most suitable feature fusion method is determined within different ranges of the number of point, line, and surface matching. The boundary values ​​of different modes are approximately obtained by the minimum and maximum number of point, line, and surface feature matchings obtained from the system operation.

[0025] Finally, based on the average number of matching points, lines and surfaces in the scene for the four fusion modes, the range that is closer to the average number of matching features of the fusion mode is expanded to the corresponding mode.

[0026]

[0027] Where, n p n l n π V1, V2, V3, and V4 represent the number of point, line, and surface feature matches, respectively, and correspond to the regions of the final result.

[0028] Furthermore, in step 3, for point features, we construct their least squares function using a reprojection method:

[0029]

[0030] Among them, u i P represents the feature point corresponding to the 3D point in the current frame. i Represents a 3D point in space; K represents the camera intrinsic parameters, T cw This represents the transformation matrix from the world coordinate system to the camera coordinate system; the corresponding Jacobian matrix is:

[0031]

[0032] For line features, we project the 3D line in space onto the current frame, calculate the distance from the endpoint of the projected line to the corresponding 2D line feature in the current frame, and treat this distance as the reprojection error of the line feature:

[0033] e L =l T KT cw P

[0034] Where K represents the camera intrinsic parameters, T cw Let P represent the transformation matrix between the world coordinate system and the camera coordinate system; let l represent the endpoints of the 3D line segment; and let l represent the coefficients of the 2D line equation. For line features, we calculate their normalized line equation:

[0035]

[0036] Among them, l s , l e These are the starting and ending points of the line, respectively; formula e L =l T KT cw The Jacobian matrix of P is as follows:

[0037]

[0038] For surface features, since the Hessian form of a plane has four parameters, but a plane in three-dimensional space only has three degrees of freedom, overparameterization can occur. We resolve this by representing the unit normal vector with φ and ψ, where φ and ψ are the azimuth and elevation angles of the normal, respectively, thus transforming the plane representation into a minimized parametric form with only three parameters. Therefore, the plane is represented as:

[0039]

[0040] Construction reprojection error:

[0041]

[0042] Where, π m For the observation value of the corresponding plane in the current frame, π w A 3D plane in the world coordinate system; T cw This is the transformation matrix from the world coordinate system to the camera coordinate system; the corresponding Jacobian matrix is ​​as follows:

[0043]

[0044] Finally, combining the mode selection in step 2, the current camera pose is calculated, as shown in the following formula:

[0045]

[0046] Among them, H p H l H π Robust kernel functions corresponding to points, lines, and surfaces, respectively, Γ p , Γ l , Γ π These correspond to the covariance matrices of points, lines, and surfaces, respectively.

[0047] The beneficial effects of this invention are as follows:

[0048] This invention proposes a VSLAM method based on adaptive fusion of point, line, and surface features. This method aims to address the problem that traditional VSLAM cannot be used in textureless or weakly textured environments, such as indoor environments with uneven texture richness. Based on the proposed method, VSLAM can adaptively select a point, line, and surface feature fusion strategy according to texture richness in indoor environments, thereby solving the problems of tracking and localization failures and improving the operating efficiency of the VSLAM system.

[0049] This invention relates to a method for localization and mapping based on the extraction and matching results of point, line and surface features in an image, and adaptively selecting a feature fusion strategy. Specifically, it selects an appropriate method based on the richness of texture, thereby solving the problem that traditional visual SLAM cannot be used in textureless or weakly textured environments, while improving its operating efficiency. Attached Figure Description

[0050] Figure 1 Here is the overall flowchart of the algorithm;

[0051] Figure 2 This is a schematic diagram of point feature extraction and matching;

[0052] Figure 3 This is a schematic diagram of line feature extraction and matching;

[0053] Figure 4 This is a schematic diagram of planar feature extraction;

[0054] Figure 5 Distribution map of different fusion modes;

[0055] Figure 6 This is a map showing the distribution of the expanded fusion mode ranges.

[0056] Figure 7 A schematic diagram of point feature reprojection error;

[0057] Figure 8 This is a schematic diagram of the reprojection error of the line feature. Detailed Implementation

[0058] The present invention will now be further described with reference to the accompanying drawings.

[0059] Step 1: Feature Extraction and Matching

[0060] Oriented and Rotated Brief (ORB) feature points are extracted from the image, and the BRIEF descriptor is used to describe these feature points, enabling point feature matching between adjacent image frames. The point feature extraction and matching are as follows: Figure 2 As shown.

[0061] Line detection in the image is performed using the Line-Segment-Detector (LSD) method, followed by line feature matching using line segment descriptors. The line feature extraction and matching results are shown below. Figure 3 .

[0062] Planar features are extracted using a hierarchical clustering method. First, the point cloud in the image is uniformly distributed into N nodes, and nodes with insufficient or discontinuous depth information are removed. Then, the feature values ​​of each pixel in the image are clustered, with consecutive blocks whose feature differences are within a threshold range clustered into the same segmentation block. Finally, the output planar parameters are iteratively optimized for each pixel to extract planar features from the image. The planar extraction results are shown below. Figure 4 After extracting the planar features, we calculate the angle between the normal vectors of the two planes and the distance between them. When the angle between the normal vectors is less than θ and the distance between the two planes is less than d, the two planes are considered to be matched.

[0063] Step 2: Mode Selection

[0064] After obtaining the matching relationship between point, line and surface features between adjacent frames, the mode selection begins based on the matching relationship between point, line and surface features.

[0065] First, we divided our system into four modes (point, point-line, point-plane, and point-line-plane) and ran them in multiple scenarios. We obtained the minimum, maximum, and average number of point, line, and plane feature matches for each scenario. We then obtained the root mean square error (RMSE) of the running trajectory for the four modes in different scenarios, calculated as shown in Equation (1), as well as the total system running time (tt) and the average tracking time per frame (tm).

[0066]

[0067] in, The keyframe trajectory represented by x is estimated by the system. i This represents the actual camera pose.

[0068] After obtaining the required data, we use an evaluation criterion value, Performance(P), to determine which fusion method among the four modes is the best in a given scene. The evaluation value (P) is calculated as shown in Equation (2). The total system runtime, average runtime per frame, and root mean square error of the trajectory are selected for evaluation, with weights assigned as α, β, and γ, respectively.

[0069]

[0070] By comparing the values ​​of P, the most suitable feature fusion method was determined within different ranges of point, line, and surface matching numbers. The statistical results are as follows: Figure 5 As shown in the figure, the boundary values ​​of different modes are approximately obtained by using the minimum and maximum number of matching points, lines, and surfaces obtained from the system operation.

[0071] Finally, for Figure 5 The blank areas in the image are expanded. Based on the average number of matching points, lines, and surfaces in the scene according to the four fusion modes, the expansion is performed, and the intervals closer to the average number of matching features of the fusion mode are expanded to the corresponding mode. The final result is as follows: Figure 6 As shown. It can also be expressed by equation (3):

[0072]

[0073] Where, n p n l n π These represent the number of point, line, and surface feature matches, respectively. V1, V2, V3, and V4 correspond to... Figure 6 The area in.

[0074] Step 3: Pose Estimation

[0075] After extracting the point, line, and surface features required by the system, pose estimation begins. For point features, we use a reprojection method (see diagram below). Figure 7 (As shown). Construct its least squares function:

[0076]

[0077] Among them, u i P represents the feature point corresponding to the 3D point in the current frame. i Represents a 3D point in space. K represents the camera intrinsic parameter, T... cw This represents the transformation matrix from the world coordinate system to the camera coordinate system. The Jacobian matrix corresponding to formula (4) is:

[0078]

[0079] For line features, we project the 3D line in space onto the current frame, calculate the distance from the endpoint of the projected line to the corresponding 2D line feature in the current frame, and treat this distance as the reprojection error of the line feature (see diagram of reprojection). Figure 8 As shown):

[0080] e L =l T KT cw P (6)

[0081] Where K represents the camera intrinsic parameters, T cwLet P represent the transformation matrix from the world coordinate system to the camera coordinate system, and let l represent the endpoints of the 3D line segment. Let l represent the coefficients of the 2D line equation. For line features, we calculate their normalized line equation:

[0082]

[0083] Among them, l s , l e These represent the starting and ending points of the line, respectively. The Jacobian matrix of formula (6) is as follows:

[0084]

[0085] For surface features, since the Hessian form of a plane has four parameters, but a plane in three-dimensional space only has three degrees of freedom, overparameterization can occur. We transform the plane representation into a minimized parametric form with only three parameters by representing the unit normal vector with φ and ψ (φ and ψ are the azimuth and elevation angles of the normal, respectively). Therefore, a plane can be represented as:

[0086]

[0087] Construction reprojection error:

[0088]

[0089] Where, π m For the observation value of the corresponding plane in the current frame, π w T is a 3D plane in the world coordinate system. cw Let be the transformation matrix from the world coordinate system to the camera coordinate system. The Jacobian matrix corresponding to formula (10) is as follows:

[0090]

[0091] Finally, combining the mode selection in step 2, the current camera pose is calculated, as shown in the following formula:

[0092]

[0093] Among them, H p H l H π Robust kernel functions corresponding to points, lines, and surfaces, respectively, Γ p , Γ l , Γ π These correspond to the covariance matrices of points, lines, and surfaces, respectively.

[0094] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. An indoor visual SLAM method based on adaptive fusion of point, line, and surface features, characterized in that: The specific steps are as follows: Step 1: Feature extraction and matching; Extract ORB feature points from the image, describe the feature points using BRIEF descriptors, and achieve point feature matching between adjacent image frames; Line detection in the image is performed using the LSD method, and then line feature matching is performed using line segment descriptors; Surface features are extracted using hierarchical clustering. Step 2: Mode selection; After obtaining the matching relationship between point, line, and surface features between adjacent frames, the mode selection begins based on the matching relationship between point, line, and surface features. First, the system is divided into four modes: point, point-line, point-plane, and point-line-plane. It is run in multiple scenarios to obtain the minimum, maximum, and average number of point, line, and plane feature matching for each scenario. The root mean square error (RMSE) of the running trajectory of the four modes in different scenarios is obtained. The calculation method is as follows, as well as the total system running time tt and the average tracking time per frame tm. in, The keyframe trajectory represents the system's estimated value. This represents the actual camera pose; After obtaining the required data, we determine which fusion method among the four modes is the best in a given scenario by setting an evaluation criterion value P. The evaluation value P is calculated as follows: The evaluation criteria include the total system runtime, average runtime per frame, and root mean square error of the trajectory. The weights assigned are as follows: , , ; By comparing the value of P, the most suitable feature fusion method is determined within different ranges of the number of point, line, and surface matching. The boundary values ​​of different modes are approximately obtained by the minimum and maximum number of point, line, and surface feature matchings obtained from the system operation. Finally, based on the average number of matching points, lines and surfaces in the scene for the four fusion modes, the range that is closer to the average number of matching features of the fusion mode is expanded to the corresponding mode. in, , , These represent the number of point, line, and surface feature matches, respectively. , , , These correspond to the regions of the final result; Step 3: Pose estimation; After extracting the point, line, and surface features required by the system, pose estimation begins.

2. The indoor visual SLAM method based on adaptive fusion of point, line, and surface features according to claim 1, characterized in that: In step 1, surface features are extracted using hierarchical clustering. First, the point cloud in the image is evenly distributed into N nodes, and nodes with true or discontinuous depth information are removed. Then, the feature values ​​of each pixel in the image are clustered, and continuous blocks with feature differences within a threshold range are clustered into the same segmentation block. Finally, the output plane parameters of each pixel are iteratively optimized to extract the planar features from the image. After extracting the planar features, we calculate the angle between the normal vectors of the two planes and the distance between the two planes. When the angle between the normal vectors is less than θ and the distance between the two planes is less than d, the two planes are considered to be matched.

3. The indoor visual SLAM method based on adaptive fusion of point, line, and surface features according to claim 1, characterized in that: In step 3, for point features, we construct their least squares function using a reprojection method: in, This represents the feature point corresponding to a 3D point in the current frame. Represents a 3D point in space; Indicates camera intrinsic parameters. This represents the transformation matrix from the world coordinate system to the camera coordinate system; the corresponding Jacobian matrix is: For line features, we project the 3D line in space onto the current frame, calculate the distance from the endpoint of the projected line to the corresponding 2D line feature in the current frame, and treat this distance as the reprojection error of the line feature: in, Indicates camera intrinsic parameters. The transformation matrix representing the world coordinate system versus the camera coordinate system. Indicates the endpoints of a 3D line segment; The coefficients represent the equation of a 2D line. For line characteristics, we calculate its normalized equation: in, , These are the starting and ending points of the line, respectively; formula The Jacobian matrix is ​​as follows: For surface features, since a plane represented in Hessian form has four parameters, but a plane in three-dimensional space only has three degrees of freedom, overparameterization can occur. We address this by using the unit normal vector... and express, and These are the azimuth and elevation angles of the normal, respectively, transforming the plane representation into a minimized parametric form with only three parameters; therefore, the plane is represented as: Construction reprojection error: in, For the observations of the corresponding plane in the current frame, A 3D plane in the world coordinate system; Let be the transformation matrix from the world coordinate system to the camera coordinate system; the corresponding Jacobian matrix is ​​as follows: Finally, combining the mode selection in step 2, the current camera pose is calculated, as shown in the following formula: in, , , Robust kernel functions corresponding to points, lines, and surfaces, respectively. , , These correspond to the covariance matrices of points, lines, and surfaces, respectively.

Images