RGB-D visual inertia SLAM system based on point-line-surface feature fusion

Abstract

The invention provides an RGB-D visual inertia SLAM (Simultaneous Localization and Mapping) system fusing point, line and surface features, which is used for positioning and mapping in an indoor environment. The system uses an RGB-D camera to extract point features and line features from a color map, and extracts plane features from a depth map. And the three are associated together by using the corresponding relationship between point-line and point-surface. According to the method, lines with specific constraints and planes with orthogonal structures are obtained through correlation matching and structured constraints, so that the number of features required during tracking is reduced, the complexity of BA optimization is reduced, and the tracking efficiency is improved. The problem that an existing SLAM system based on a traditional single feature or simple feature combination is poor in precision and robustness under the conditions that scene features are few and noise interference is serious is solved.

Description

Technical Field

[0001] The present invention discloses an RGB-D visual inertial SLAM system based on point, line and surface feature fusion, belonging to the field of positioning and mapping. Background Art

[0002] The core goal of SLAM (Simultaneous Localization and Mapping) is to allow robots or autonomous vehicles to autonomously locate in unknown environments and build environmental maps at the same time; it is an important technology, especially in indoor and tunnel scenarios where GPS signals are unavailable or unreliable; Visual Inertial SLAM is a SLAM technology that combines visual sensors and inertial measurement units to solve the problem of positioning and mapping in dynamic or complex environments. It is a core technology in the fields of autonomous driving, drone navigation, augmented reality, etc.

[0003] Although most existing visual-inertial systems have good performance in solving external interference and increasing system robustness, there are still many deficiencies in practical applications. Visual-inertial odometers have the problem of cumulative drift error, and visual-inertial SLAM based on feature points has computational difficulties when optimizing sparse points, key frame poses, and IMU states, and cannot meet real-time requirements. Summary of the invention

[0004] The purpose of the present invention is to propose an RGB-D visual-inertial SLAM system based on point-line-surface feature fusion, which enhances positioning accuracy and mapping accuracy by introducing advanced features, and improves the robustness of the system by combining inertial information.

[0005] The present invention uses an RGB-D camera to extract point features and line features from a color image, extracts plane features from a depth image, and uses the correspondence between point-line and point-plane to associate the three together. The present invention obtains lines with specific constraints and planes with orthogonal structures through associative matching and structured constraints to reduce the number of features required for tracking and reduce the complexity of BA optimization. The present invention adds constraints to plane features by extracting plane edges to generate assumed vertical planes, thereby obtaining real and effective plane features in indoor environments and enhancing positioning accuracy.

[0006] The RGB-D visual inertial SLAM system based on point, line and surface feature fusion of the present invention includes the following contents:

[0007] 1. Read the color image, depth image, and inertial information obtained by the sensor, perform preliminary filtering and screening on the acquired data, remove inappropriate data, and input the processed data into the Tracking thread.

[0008] 2. The Tracking thread extracts various features from the incoming data, calculates the current posture, matches the associated feature data, minimizes the error of the corresponding matching features through reprojection calculation, and sends all the extracted feature information to the Local Mapping thread.

[0009] 3. The Local Mapping thread completes operations such as adding keyframes, point features, line features, and plane features to the active map, removing redundant keyframes and map points, and using visual inertial navigation BA to optimize the map.

[0010] 4. After the Local Mapping thread is completed, the Looping and Global Optimization thread is entered. This thread detects the common area between the active map and the entire Atlas at the key frame rate. If the common area belongs to the active map, loop correction is performed; if the common area belongs to different maps, the two maps are seamlessly merged into one map, which becomes the active map. After completing the loop correction and map merging, a separate thread is started to perform global BA optimization to obtain the optimal global map. BRIEF DESCRIPTION OF THE DRAWINGS

[0011] Figure 1 This is a schematic diagram of the overall framework of the RGB-D visual-inertial SLAM system based on point, line and surface feature fusion.

[0012] Figure 2 This is a schematic diagram of the triangulation of the Plücker coordinate lines.

[0013] Figure 3 It is a schematic diagram of the spatial point fitting plane. DETAILED DESCRIPTION

[0014] In order to more clearly describe the technical content of the present invention, the invention content is further described below in conjunction with specific implementations.

[0015] Image information and inertial information are obtained from the RGB-D sensor and input into the Tracking thread. The Tracking thread extracts various features, calculates the current posture, and minimizes the error of the corresponding matching features through reprojection calculation.

[0016] The ORB feature extraction method is used to detect key points, a multi-scale image pyramid is constructed, directions are assigned to the detected key points, and the BRIEF descriptor is used to describe the local image features of the key points.

[0017] The improved LSD algorithm is used to extract line features, and the line features are represented by Plücker coordinates, and the line features in the world coordinate system are projected into the image coordinate system: Where Lw and Lc are the Plücker coordinates of the line features in the world coordinate system and the image coordinate system respectively, and Tcw is the projection matrix.

[0018] Minimize the reprojection error of the line features in the above image coordinate system: where l = [l1,l2,l3] T =KLmc is the projection line of Lc, KL is the straight line projection matrix.

[0019] The length suppression strategy is used to select the minimum threshold to select the line feature of appropriate length. The minimum threshold satisfies: Where min(W,H) represents the minimum value between the length and width of the input image, and η is the ratio factor, which is generally set to 0.125.

[0020] Through segmentation, fitting and merging operations, with the help of the relationship between three-dimensional points and planes, the plane features are reconstructed from the point cloud data by minimizing the distance using geometric relationships, and a global map composed of plane features is constructed.

[0021] Using the Hessian form π=(nT,d) T The plane feature is represented parameterized, n is the normal vector of the plane, and d is the distance from the plane to the origin of the coordinate system.

[0022] With the help of the relationship between the pixels in the depth map and the distance of the objects in the color map, the pinhole camera model is used to back-project the pixels to restore the spatial structure: Where (u, v) is the effective pixel in the depth image, d is the depth value, (x, y, z) is the corresponding 3D point coordinate, fx, fy is the camera focal length, and cx, cy is the camera center coordinate.

[0023] Segment the point cloud data in the object space structure restored in the previous step, and use the idea of ​​RANSAC algorithm to perform plane fitting using non-collinear 3D space points. The specific steps are as follows:

[0024] Randomly select three different points p1, p2, p3, and calculate the vectors p1p2 = (l, m, n), p1p3 = (o, p, q), then the normal vector nT of the plane can be expressed as: nT=p1p2× p1p3=(mq-np,no-lq,lp-om) (5)

[0025] By setting a minimum distance threshold to divide the inner and outer points, the distance from the spatial point to the preliminary fitting plane is calculated, and all the small planes of the inner point fitting that meet the threshold are merged together to obtain a larger plane.

[0026] The plane represented by the Hessian form is an infinite plane. By extracting the edge lines of the plane and generating an assumed vertical plane, edge constraints are added to it to obtain plane features that are valid in real scenes.

[0027] Extract edge lines from the contour boundary of the plane. When a line has more than 20% of the boundary contour points, it is considered to be an edge line. The edge line can be expressed as: in It's a point on the edge line. is the direction vector of the edge line.

[0028] The extracted edge lines and the planes they belong to are used to calculate and generate a hypothetical vertical plane. and edge strips To represent the hypothetical plane: where πsup is the generated hypothetical plane.

[0029] The feature information extracted by the Tracking thread is sent to the Local Mapping thread, which completes operations such as adding keyframes, point features, line features, and plane features to the active map, removing redundant keyframes and map points, and using visual inertial navigation BA to optimize the map.

[0030] In the local BA optimization of visual inertial navigation, the reprojection error of points and lines is minimized. At the same time, the state of IMU is also optimized. The optimization objective function is as follows: Where i, j, z, k represent the camera view, 3D point, 3D line, and IMU information, respectively. x is the full state vector related to point, line, and IMU, epoint represents the reprojection residual of the point, eline represents the reprojection residual of the line, and eimu represents the IMU measurement residual. ρ is the Huber robust cost function, W ij ,W iz ,W ik They represent the covariance matrix related to the scale of the detected feature points or feature lines and the measurement covariance matrix related to the IMU, respectively.

[0031] In the local BA optimization of visual inertial navigation, the optimization of the plane is put together with the optimization of the pose. Define the calculation function for solving the pose corresponding to the plane: Where H is the Huber robust cost function and f is the constraint factor.

[0032] After the Local Mapping thread is completed, the Looping and Global Optimization thread enters. This thread detects the common area between the active map and the entire Atlas at the key frame rate. If the common area belongs to the active map, loop correction is performed; if the common area belongs to different maps, the two maps are seamlessly merged into one map, which becomes the active map.

[0033] After completing loop correction and map merging, global BA optimization is performed in a separate thread to obtain the optimal global map.

Claims

1. The RGB-D visual inertial SLAM system based on the fusion of point, line and surface features can stably achieve positioning and tracking in the case of fewer scene features and severe noise interference, solving the problem of poor accuracy and robustness of traditional SLAM systems; the system integrates point features, line features, surface features and inertial information, and improves optimization efficiency by adding specific constraints. It mainly includes the following steps: 1) Obtain image information and inertial information from the RGB-D sensor and input it into the Tracking thread. The Tracking thread extracts various features, calculates the current posture, and minimizes the error of the corresponding matching features through reprojection calculation; 2) For feature point extraction, ORB feature extraction method is used to detect key points and construct multi-scale image fonts. Tower, and assign directions to the detected key points, using BRIEF descriptors to describe the local image features of the key points; 3) The improved LSD algorithm is used to extract line features. The calculation steps are as follows: 4) Use Plücker coordinates to represent line features and project the line features in the world coordinate system into the image coordinate system: Where L w With L c are the Plücker coordinates of the line features in the world coordinate system and the image coordinate system, respectively. cw is the projection matrix; 5) Minimize the reprojection error of the line features in the above image coordinate system: in YesL c The projection line of K L is the rectilinear projection matrix; 6) Use the length suppression strategy to select the minimum threshold to select the line feature of appropriate length. The minimum threshold satisfies: Where min(W,H) represents the minimum value between the length and width of the input image, and η is the ratio factor, which is generally set to 0.125; 7) Plane features are obtained from point cloud data; through segmentation, fitting and merging operations, with the help of the relationship between 3D points and planes, the plane features are reconstructed by minimizing the distance using geometric relationships, and a global map composed of plane features is constructed; 8) Using the Hessian form π=(n T ,d) T The plane feature is parametrically represented, n is the normal vector of the plane, and d is the distance from the plane to the origin of the coordinate system; 9) The pixels in the depth map are related to the distance of the objects in the color map. The spatial structure is restored by back-projecting the pixels using the camera model. The camera model used is the pinhole model: Where (u, v) is the effective pixel in the depth image, d is the depth value, (x, y, z) is the corresponding 3D point coordinate, and f x ,f y is the focal length of the camera, c x ,c y is the camera center coordinate; 10) Segment the point cloud data in the object space structure restored in the previous step, and use the idea of ​​RANSAC algorithm to perform plane fitting using non-collinear 3D space points; the simplest fitting only requires 3 non-collinear space points to complete, and the specific steps are as follows: 11) Randomly select three different points p1, p2, p3, and calculate the two intersecting vectors p1p2 = (l, m, n), p1p3 = (o, p, q), then the normal vector n of the plane T It can be expressed as: n T =p1p2×p1p3=(mq-np,no-lq,lp-om) (5) 12) Divide the inner and outer points by setting the minimum distance threshold, calculate the distance from the spatial point to the preliminary fitting plane, and merge all the small planes of the inner point fitting that meet the threshold together to obtain a larger plane; 13) The plane represented by the Hessian form is an infinite plane, and the plane of the real scene is often finite. Therefore, we need to add edge constraints to the plane to ensure that the obtained plane features are real and valid; 14) Extract edge lines from the contour boundary of the plane. When the line has more than 20% of the contour points, the line is considered valid; an edge line can be expressed as: in It's a point on the edge line. is the direction vector of the edge line; 15) Generate an assumed vertical plane by using the extracted edge line and its corresponding plane; and edge strips To represent the hypothetical plane: where π sup is the generated hypothetical plane; 16) All feature information obtained in the Tracking thread is sent to the Local Mapping thread, which completes operations such as adding keyframes, point features, line features, and plane features to the active map, removing redundant keyframes and map points, and using visual inertial navigation BA to optimize the map; 17) In the local BA optimization, the reprojection error of points and lines is minimized; at the same time, the state of the IMU is optimized; the optimization objective function is as follows: Where i, j, z, and k represent the number of camera viewpoints, 3D points, 3D lines, and IMU information, respectively; is the full state vector related to points, lines, and IMU, e point Represents the reprojection residual of the point, e line represents the reprojection residual of the line, e imu Represents the IMU measurement residual; ρ is the Huber robust cost function, W ij ,W iz ,W ik Respectively represent the covariance matrix related to the scale of the detected feature points or feature lines and the measurement covariance matrix related to the IMU; 18) In local BA optimization, the optimization of the plane is put together with the optimization of the posture; the calculation function for solving the posture corresponding to the plane is defined: Where H is the Huber robust cost function and f is the constraint factor; 19) After completing the Local Mapping thread, enter the Looping and Global Optimization thread; This thread detects the common area between the active map and the entire Atlas at a key frame rate. If the common area belongs to the active map, loop correction is performed; if the common area belongs to different maps, the two maps are seamlessly merged into one map. The map becomes an active map; 20) After completing loop correction and map merging, perform global BA optimization in a separate thread to obtain the optimal global map.

Images