CN110060277A - A kind of vision SLAM method of multiple features fusion - Google Patents

Abstract

The visual SLAM method of multi-feature fusion involves the field of robot visual localization and mapping. The invention discloses a multi-feature fusion visual SLAM method based on a depth camera, which solves the problem of visual positioning in the case of failure of pure point features by fully using point and line features extracted from images and constructing plane features according to the point and line features. An adaptive threshold method is used to extract point features to obtain more uniform point features; line features are extracted, short and small line segments are removed, and segmented line segments are merged to improve the accuracy of line feature matching; point and line features are used for inter-frame bits Attitude estimation and local map construction; the minimum parameter method is used to calculate surface features to reduce the amount of calculation; by constructing the back-projection error function of the fusion features, the point, line and surface features are tightly coupled, and a global map is constructed for global pose optimization. The present invention is a visual SLAM method with high precision, good real-time performance and strong robustness, and solves the problem that the precision of visual SLAM based on the feature point method decreases or even the system fails in a low texture environment.

Description

A Visual SLAM Method for Multi-feature Fusion

Technical field:

The invention belongs to the technical field of simultaneous positioning and map construction of robots, and in particular relates to a multi-feature fusion SLAM (simultaneous positioning and map construction) method.

Background technique:

With the development of visual SLAM technology, frame-based optimization and graph optimization frameworks have become mainstream frameworks for visual SLAM problems. The graph optimization framework introduces motion estimation and beam adjustment into visual SLAM. The location and surrounding environment features of the image are solved as a global optimization problem. The feature tracking is performed by extracting the features on the image, and the error function is established. The linear optimization is constructed through the linear assumption or the nonlinear optimization is directly performed to solve the error function when the minimum value is obtained. The robot pose simultaneously optimizes the landmarks. The earlier Structure from Motion (SFM) consumes too much time in feature extraction and matching and subsequent optimization, so it can only perform offline pose optimization and 3D reconstruction, and cannot complete its own positioning and map online in real time. Construct. With the discovery of the sparsity of the beam adjustment method and the upgrading of computer hardware in recent years, the time consumed by each link has been greatly reduced. Therefore, the visual SLAM based on the graph optimization framework can realize real-time positioning and mapping.

Visual SLAM is mainly divided into two categories: gray-based direct methods and feature-based indirect methods. Among them, the direct method based on gray level performs well in some cases. It does not need to perform feature extraction and matching, but only needs to perform gray gradient tracking, so it will save a lot of computing time, and this method can be used in areas with fewer features. Able to enhance the continuity of visual SLAM. However, at the same time, since the direct method only uses the gradient of pixels to perform inter-frame matching and motion tracking, this method is strongly affected by light intensity and noise interference. Although the feature-based indirect method will occupy some computing resources, due to the improvement of the hardware level, the computing resources occupied are greatly reduced, and the extracted features are artificially designed, with relatively stable characteristics, and can perform feature tracking, composition, Closed-loop detection can complete the whole process of simultaneous positioning and map construction of the robot, so the feature-based visual SLAM method has gradually become the mainstream of SLAM technology research.

Invention content:

The present invention is to solve the problems that the existing point feature-based visual SLAM algorithm cannot perform stable tracking on the point feature calculated in the low-texture environment, and the constraint force generated according to the point feature decreases, and an accurate environment map cannot be constructed. The invention provides a multi-feature fusion visual SLAM method.

First model the SLAM problem as:

is a six-dimensional vector on SE(3), representing the motion of the robot from t to t+1, Represents the covariance matrix for motion estimation, inversely approximated by the Hessian matrix of the loss function in the previous iteration.

The technical solution adopted in the present invention is: a visual SLAM method based on image point, line and surface feature fusion, which is characterized in that it includes the following steps:

Step 1, carry out the calibration of the depth camera, and determine the internal parameters of the camera;

Step 2, for the video stream data obtained by the camera on the mobile robot platform, perform Gaussian filtering to reduce the influence of noise on subsequent steps;

Step 3: Perform feature extraction on the corrected image, and extract point features and line features of each frame online, specifically including: extracting FAST corner features for each frame of image, using rBRIEF descriptor to describe, and using LSD algorithm for line features Detect and describe using the LBD descriptor, and use the geometric information provided by the line segment to screen out the line segments with different directions and lengths and large distances between the endpoints;

Step 4, construct surface features according to the extracted point and line features, and construct candidate surface features through the following three methods: 1) three coplanar feature points; 2) one feature point and one feature line; 3) two Coplanar feature lines;

Step 5, use the point and line features between the current frame and the previous frame to perform inter-frame matching of the image, obtain the matching pair of point-line features, and establish the geometric correspondence between the frames;

Step 6, perform feature back-projection according to the correspondence between frames obtained in step 5, and use the Gauss-Newton method to minimize the back-projection error of the feature. get incremental motion between two consecutive frames;

Step 7, select the key frame, and convert the uncertainty of the covariance matrix into the form of entropy by the following formula:

h(ζ)=3(1+log(2π))+0.5log(|Ω _ζ |),

Ω represents the covariance matrix of incremental motion. Calculate the ratio:

If α < 0.9, and the number of overlapping features between the current frame and the previous key frame exceeds 300, add the image to the key frame list, otherwise return to step 2 to continue processing the input image data;

Step 8, build a local map based on the idea of the graph, which consists of all key frames associated with the current frame and the features observed according to these key frames. Define ψ as a variable under se(3), including every two consecutive frames. The incremental motion of , the spatial coordinate point of each point feature, the spatial coordinate point of the line feature endpoint, and the minimum representation vector of the plane; then minimize the distance between the observation data and the road sign projection to construct an accurate local map;

Step 9: Determine whether the trajectory of the robot motion forms a closed loop. If so, modify the local maps at both ends, still using the idea of graph optimization, take the robot pose represented by the key frame as a node, and the constraint relationship between the local maps at both ends as an edge to correct the trajectory; otherwise, determine whether there is a stop signal, if no stop signal is received, return to step 2 to perform the visual SALM process, otherwise go to step 10;

Step 10, stop exercising.

Further, the implementation method of step 1:

Use the camera to obtain multiple fixed-size checkerboard image data from different viewing angles in advance, and then use Zhang Zhengyou's camera calibration method to calculate the camera's internal parameters on the obtained checkerboard image data to obtain the camera calibration result.

Further, the implementation of step 3:

Step 3.1, use the M16 template to extract the FAST corner feature of each frame image, the M16 template is as shown in accompanying drawing 2, if the pixels of 12 consecutive points are all greater than _Ip +T or less than _Ip -T, then Feature points; first calculate an adaptive threshold T:

in Represents the average value of the gray values of all pixels in the current circle; judges the relationship between the gray values of the four points 1, 5, 9, and 13 and the size of T. When at least three points meet the above conditions, continue to calculate the remaining 12 points, according to The gray-scale centroid method determines the direction of the feature points: the position C of the centroid is calculated in the neighborhood S, and the moment of the neighborhood S is defined as:

The centroid is:

The orientation of the feature points is:

θ=atan 2(m ₀₁ , m ₁₀ ),

The extracted corner features are described by the rBRIEF descriptor with rotation invariance;

Step 3.2, use the LSD algorithm to detect the possible line segment features on the image, and merge the line segments that may be the same straight line, by calculating the angle between any two line features and the distance between the midpoint of each line segment and the other line segment, if respectively If it is less than the thresholds T _lang and T _ldis , it is considered to be the same line segment, which is merged, and then the LBD algorithm is used to describe the optimized line feature to obtain the descriptor of the line feature.

Further, the implementation method of step 4:

Step 4.1, solve the spatial coordinates of the point and line features extracted in step 3. In particular, after obtaining the spatial coordinates of the line features, calculate the equation of the spatial straight line to which they belong, and then divide the features into two sets, and the point features are classified as _Sp set, line features are classified as S _l set;

Step 4.2, construct candidate surface features through the following three methods: 1) three coplanar feature points; 2) one feature point and one feature line; 3) two coplanar feature lines; Equation: p ₁ x+p ₂ y+p ₃ z=p ₄ ;

Step 4.3, normalize the coefficients of the plane equation:

||[p ₁ ,p ₂ ,p ₃ ,p ₄ ] ^T ||=1,

In this way, a plane is represented by three quantities: p=[p ₁ , p ₂ , p ₃ ] ^T ;

Further, the implementation method of step 5:

Step 5.1: Match according to the point, line, and surface features obtained in step 3. For point features, sort them in ascending order according to the distance of the descriptors, and take the first 60% as the best matching point pair; for line features, the same Sort in ascending order according to the distance of the descriptors, and take the top 50% as the best matching pair; for surface features, calculate:

Sort in ascending order, and take the first 3-5 as the best matching pair;

Step 5.2, use the RANSAC matrix to remove the error features of the obtained matching pairs;

Further, the implementation method of step 6:

Step 6.1, perform 3D reconstruction according to the matched features, obtain 3D coordinates, and then reproject according to the geometric relationship between adjacent frames, calculate the reprojection error of the feature, and use the Gauss-Newton method to minimize the projection error of points and lines;

Step 6.2, using a pseudo Huber loss function and performing a two-step minimization process, finally obtains the incremental motion between two consecutive keyframes.

Further, the implementation method of step 7:

Step 7.1, convert the uncertainty in the covariance matrix into a scalar form of entropy:

h(ζ)=3(1+log(2π))+0.5log(|Ω _ζ |),

where Ω represents the covariance matrix of incremental motion. For a given input frame image, check the entropy of the motion estimation between the previous key frame i and the current frame i+u and its entropy with the first key frame i+1 Ratio, calculate:

If the value of α is lower than a certain threshold, which is specified as 0.9 in this method, and the number of feature overlaps between the current frame and the previous key frame exceeds 300, then the i+u frame is inserted into the system as a new key frame;

In step 7.2, since ζ _{t, t+1} is only an estimate between adjacent frames, the above-mentioned series of estimates are synthesized through a first-order Gaussian transfer function to obtain the covariance between two non-consecutive frames.

Further, the implementation method of step 8:

Step 8.1, refine the estimation of the relative pose change between the current frame and all previous key frames, perform data association, and use the incremental motion obtained in step 6 as the initial input of the Gauss-Newton optimization method;

Step 8.2, define ψ as a variable under se(3), including the incremental motion between every two consecutive frames, the spatial coordinate point of each point feature, the spatial coordinate point of the line feature endpoint, and the minimum representation vector of the plane, the minimum Reprojection error:

Among them, K _l , P _l , L _l , and M _l represent the local map set, point feature set, line feature set, and surface feature set, respectively, and Ω represents the covariance matrix of incremental motion. The point feature projection error _eij is the two-dimensional distance observed by the jth mapping point in the ith frame, and is calculated according to the following formula:

e _ij =x _ij -π(ζ _iw ,X _wj ),

where π refers to the mapping from se(3) to SE(3), and _Xwj is the three-dimensional spatial coordinate of the point feature; the projection error of the line feature is different from that of the point feature, and the endpoints of the three-dimensional line segment are projected to the image. plane, and taking the distance between it and the corresponding infinite straight line on the image plane as the error function, e _ik is calculated by:

In the formula, P and Q are the endpoint coordinates of the line feature, respectively, and l _ik represents the straight line equation on the plane, which is given by step 3; the projection error e _im of the surface feature is calculated according to the logarithmic mapping of the quaternion:

e _im =log{q[T(ζ _iw ) ^T p(ζ _im )] ^-1 q(ζ _iw )},

where p(ζ _im ) represents the plane, given by step 3, q(*) is a quaternion, and T(ζ _iw ) is the homogeneous form of a transformation matrix. The second-generation update is performed using the Levenberg-Marquardt optimization method to obtain the optimal incremental motion.

Further, the implementation method of step 9:

Step 9.1, perform Brief binary description on the extracted line features, obtain the descriptors of the line features, and the descriptors of the point features to form the visual term vector of the current frame, and perform matching according to the visual word bag model established offline in advance;

Step 9.2, if the matching is successful, it is necessary to optimize and merge the two local maps where the two key frames are located, and update the local maps related to the two local maps. Based on the graph optimization framework, the incremental motion between frames is As nodes of the graph, edges are constructed similarly to step 8, according to:

To perform iterative update, the log and exp functions respectively represent the logarithmic mapping from Lie groups to Lie algebras and the exponential mapping from Lie algebras to Lie groups;

Step 9.3, if the match is unsuccessful, judge whether there is a stop signal, if no stop signal is received, return to step 2 to continue running, otherwise go to step 10.

The present invention has the following beneficial effects:

1. The use of multiple features for tight coupling can effectively solve the problems of sharp decline in the accuracy of the feature-based system and system failure in a low-texture environment;

2. It can realize the positioning effect of the mobile platform and build the surrounding environment features with structural information according to the indoor structural environment. It can obtain high-precision results on a variety of public experimental data sets, and can be used in real time and efficiently. The matched point, line and surface features are used to compose the pose of the mobile platform and the surrounding environment, and perform loop closure detection processing, making full use of the point and line features to reduce accumulated errors.

Description of drawings:

1 is a schematic flowchart of a visual SLAM method for multi-feature fusion according to the specific embodiment;

Figure 2 is a schematic diagram of the FAST feature M16 template used in the SLAM method in this paper;

Figure 3 shows the probabilistic graph model based on the idea of graph optimization used in the SLAM method in this paper;

Figure 4 is a schematic diagram of the plane features used in the SLAM method in this paper;

Figure 5 is a schematic diagram of the re-projection of point-line features in the SLAM method in this paper.

Detailed ways:

The present invention will be further described below with reference to the accompanying drawings and specific embodiments.

The technical solution adopted in the present invention is: a visual SLAM method based on multi-feature fusion, and the specific implementation process is shown in Figure 1, which mainly includes the following steps:

Step 1, carry out the calibration of the depth camera, and determine the internal parameters of the camera;

Step 2, for the video stream data obtained by the camera on the mobile robot platform, perform Gaussian filtering to reduce the influence of noise on subsequent steps;

Step 3: Perform feature extraction on the corrected image, and extract point features and line features of each frame online, specifically including: extracting FAST corner features for each frame of image, using rBRIEF descriptor to describe, and using LSD algorithm for line features Detect and describe using the LBD descriptor, and use the geometric information provided by the line segment to screen out the line segments with different directions and lengths and large distances between the endpoints;

Step 4, construct surface features according to the extracted point and line features, and construct candidate surface features through the following three methods: 1) three coplanar feature points; 2) one feature point and one feature line; 3) two Coplanar feature lines;

Step 5, use the point and line features between the current frame and the previous frame to perform inter-frame matching of the image, obtain the matching pair of point-line features, and establish the geometric correspondence between the frames;

Step 6, perform feature back-projection according to the correspondence between frames obtained in step 5, and use the Gauss-Newton method to minimize the back-projection error of the feature. get incremental motion between two consecutive frames;

Step 7, select the key frame, and convert the uncertainty of the covariance matrix into the form of entropy through the formula --------:

h(ζ)=3(1+log(2π))+0.5log(|Ω _ζ |),

Calculate the ratio:

If α < 0.9, add the image to the key frame list, otherwise return to step 2 to continue processing the input image data;

Step 8, build a local map based on the idea of the graph, which consists of all key frames associated with the current frame and the features observed according to these key frames. Define ψ as a variable under se(3), including every two consecutive frames. The incremental motion of , the spatial coordinate point of each point feature, the spatial coordinate point of the line feature endpoint, and the minimum representation vector of the plane; then minimize the distance between the observation data and the road sign projection to construct an accurate local map;

Step 9: Determine whether the trajectory of the robot motion forms a closed loop. If so, modify the local maps at both ends, still using the idea of graph optimization, take the robot pose represented by the key frame as a node, and the constraint relationship between the local maps at both ends as an edge to correct the trajectory; otherwise, judge whether there is a stop signal, if no stop signal is received, return to step 2 to continue the visual SALM process, otherwise go to step 10;

Step 10, stop exercising.

Further, the implementation method of step 1:

Use the camera to obtain multiple fixed-size checkerboard image data from different viewing angles in advance, and then use Zhang Zhengyou's camera calibration method to calculate the camera's internal parameters on the obtained checkerboard image data to obtain the camera calibration result.

Further, the implementation of step 3:

Step 3.1, use the M16 template to extract the FAST corner feature of each frame image, the M16 template is as shown in accompanying drawing 2, if the pixels of 12 consecutive points are all greater than _Ip +T or less than _Ip -T, then Feature points; first calculate an adaptive threshold T:

in Represents the average value of the gray values of all pixels in the current circle; judges the relationship between the gray values of the four points 1, 5, 9, and 13 and the size of T. When at least three points meet the above conditions, continue to calculate the remaining 12 points, according to The gray-scale centroid method determines the direction of the feature points: the position C of the centroid is calculated in the neighborhood S, and the moment of the neighborhood S is defined as:

The centroid is:

The orientation of the feature points is:

θ=atan 2(m ₀₁ , m ₁₀ ),

The extracted corner features are described by the rBRIEF descriptor with rotation invariance;

Step 3.2, use the LSD algorithm to detect the possible line segment features on the image, and merge the line segments that may be the same straight line, by calculating the angle between any two line features and the distance between the midpoint of each line segment and the other line segment, if respectively If it is less than the thresholds T _lang and T _ldis , it is considered to be the same line segment, which is merged, and then the LBD algorithm is used to describe the optimized line feature to obtain the descriptor of the line feature.

Further, the implementation method of step 4:

Step 4.1, solve the spatial coordinates of the point and line features extracted in step 3. In particular, after obtaining the spatial coordinates of the line features, calculate the equation of the spatial straight line to which they belong, and then divide the features into two sets, and the point features are classified as _Sp set, line features are classified as S _l set;

Step 4.2, construct candidate surface features through the following three methods: 1) three coplanar feature points; 2) one feature point and one feature line; 3) two coplanar feature lines; Equation: p ₁ x+p ₂ y+p ₃ z=p ₄ ;

Step 4.3, normalize the coefficients of the plane equation:

||[p ₁ ,p ₂ ,p ₃ ,p ₄ ] ^T ||=1,

In this way, three quantities are used to represent a plane: [p ₁ , p ₂ , p ₃ ] ^T ;

Further, the implementation method of step 5:

Step 5.1: Match according to the point, line, and surface features obtained in step 3. For point features, sort them in ascending order according to the distance of the descriptors, and take the first 60% as the best matching point pair; for line features, the same Sort in ascending order according to the distance of the descriptors, and take the top 50% as the best matching pair; for line features, calculate:

Sort in ascending order, and take the first 3-5 as the best matching pair;

Step 5.2, use the RANSAC matrix to remove the error features of the obtained matching pairs;

Further, the implementation method of step 6:

Step 6.1, perform 3D reconstruction according to the matched features, obtain 3D coordinates, and then reproject according to the geometric relationship between adjacent frames, calculate the reprojection error of the feature, and use the Gauss-Newton method to minimize the projection error of points and lines;

Step 6.2, using a pseudo Huber loss function and performing a two-step minimization process, finally obtains the incremental motion between two consecutive keyframes.

Further, the implementation method of step 7:

Step 7.1, convert the uncertainty in the covariance matrix into a scalar form of entropy:

h(ζ)=3(1+log(2π))+0.5log(|Ω _ζ |),

For a given input frame image, check the entropy of the motion estimation between the previous key frame i and the current frame i+u and its ratio to the first key frame i+1, calculate:

If the value of α is lower than a certain threshold, which is specified as 0.9 in this method, and the number of feature overlaps between the current frame and the previous key frame exceeds 300, then the i+u frame is inserted into the system as a new key frame;

In step 7.2, since ζ _{t, t+1} is only an estimate between adjacent frames, the above-mentioned series of estimates are synthesized through a first-order Gaussian transfer function to obtain the covariance between two non-consecutive frames.

Further, the implementation method of step 8:

Step 8.1, refine the estimation of the relative pose change between the current frame and all previous key frames, perform data association, and use the incremental motion obtained in step 6 as the initial input of the Gauss-Newton optimization method;

Step 8.2, define ψ as a variable under se(3), including the incremental motion between every two consecutive frames, the spatial coordinate point of each point feature, the spatial coordinate point of the endpoint of the line feature, and the minimum representation vector of the plane, the minimum Reprojection error:

Among them, K _l , P _l , L _l , and M _l represent local map sets, point feature sets, line feature sets, and surface feature sets, respectively. The point feature projection error _eij is the two-dimensional distance observed by the jth mapping point in the ith frame, and is calculated according to the following formula:

e _ij =x _ij -π(ζ _iw ,X _wj ),

where π refers to the mapping from se(3) to SE(3), and _Xwj is the three-dimensional spatial coordinate of the point feature; the projection error of the line feature is different from that of the point feature, and the endpoints of the three-dimensional line segment are projected to the image. plane, and taking the distance between it and the corresponding infinite straight line on the image plane as the error function, e _ik is calculated by:

In the formula, P and Q are the endpoint coordinates of the line feature, respectively, and l _ik represents the straight line equation on the plane, which is given by step 3; the projection error e _im of the surface feature is calculated according to the logarithmic mapping of the quaternion:

e _im =log{q[T(ζ _iw ) ^T p(ζ _im )] ^-1 q(ζ _iw )},

where p(ζ _im ) represents the plane, given by step 3, q(*) is a quaternion, and T(ζ _iw ) is the homogeneous form of a transformation matrix. The second-generation update is performed using the Levenberg-Marquardt optimization method to obtain the optimal incremental motion.

Further, the implementation method of step 9:

Step 9.1, perform Brief binary description on the extracted line features, obtain the descriptors of the line features, and the descriptors of the point features to form the visual term vector of the current frame, and perform matching according to the visual word bag model established offline in advance;

Step 9.2, if the matching is successful, it is necessary to optimize and merge the two local maps where the two key frames are located, and update the local maps related to the two local maps. Based on the graph optimization framework, the incremental motion between frames is As nodes of the graph, edges are constructed similarly to step 8, according to:

To perform iterative update, the log and exp functions respectively represent the logarithmic mapping from Lie groups to Lie algebras and the exponential mapping from Lie algebras to Lie groups;

Step 9.3, if the match is unsuccessful, judge whether there is a stop signal, if no stop signal is received, return to step 2 to continue running, otherwise go to step 10.

Claims (5)

1. a kind of vision SLAM method of multiple features fusion, which is characterized in that the reality of visual odometry front end and figure optimization rear end Existing method, key step are as follows:

Image preprocessing is carried out to the image of acquisition first in visual odometry front end, to reduce in initial pictures noise for spy Levy the influence of detection；FAST characteristic point is extracted in image after the pre-treatment using a kind of method of adaptive threshold, is selected Wherein Harris responds highest 500 characteristic points of score and calculates rBRIEF descriptor；The line feature for extracting image, uses LSD Algorithm extracts the line segment feature in image, for the line segment feature extracted, needs to filter out length less than 20 pixels, simultaneously It lays down a regulation, it would be possible to be merged for two line segment features of same line segment feature；It is candidate by the method construct of formulation Region feature indicates region feature using minimum planes representation；Characteristic matching is carried out to the image of adjacent two frame, obtains camera The variation of interframe pose, and judge whether present frame is key frame, it is then saved if key frame, and construct local map；

The local map of building is optimized in figure optimization rear end, is played a game position appearance using Levenberg-Marquadt algorithm It optimizes, solves optimal pose；Judge whether present incoming frame can constitute winding, to the part at both ends if it can constitute winding Map is modified, and the robot pose that key frame is represented comes as node, the constraint relationship of both ends local map as side Carry out the amendment of track.

2. a kind of vision SLAM method of multiple features fusion as described in claim 1, which is characterized in that use a kind of new side Method detects FAST feature:

Using the pixel around M16 template selection characteristic point, radius is 3 pixels, selects 10 gray values most in current circle Big and 10 the smallest pixels of gray value calculate threshold value T:

Wherein I_iIndicate the average value of all pixels gray value in current circle；Compare first in 16 points again number be 1,5,9, 13 pixel and the relationship of T, the gray scale there are three or more judge the pass of these points intermediate left point and T if being greater than T System, if more than T, then the point is characterized a little.

3. a kind of vision SLAM method of multiple features fusion as described in claim 1, which is characterized in that use minimum planes It indicates to indicate the region feature extracted:

Using point feature obtained in claim 2, line feature is extracted using LSD algorithm, dotted line feature is classified, point is special Sign is classified as S_pSet, line feature are classified as S_lSet, particularly, line feature seek calculating after its space coordinate the space belonging to it The equation of straight line goes out candidate region feature: 1) three coplanar characteristic points by following three kinds of method constructs；2) characteristic point with One characteristic curve；3) two coplanar characteristic curves；And calculate the general equation of candidate plane: p₁x+p₂y+p₃Z=p₄, and will put down The coefficient of face equation normalizes: | | [p₁,p₂,p₃,p₄]^T| |=1, plane: p=[p is indicated with three amounts with this₁,p₂, p₃]^T。

4. a kind of vision SLAM method of multiple features fusion as described in claim 1, which is characterized in that sufficiently use dotted line The re-projection error of region feature completes the optimization of local map:

Present frame is refined with the variation estimation of the relative pose between all key frames before, carries out data correlation, The incremental motion for using step 6 to obtain is as the initial input of Gauss-Newton optimization method；Defining ψ is the variable under se (3), The space coordinate point of incremental motion, each point feature including the continuous interframe of every two, line feature endpoint space coordinate point with And the minimum of plane indicates vector, minimizes re-projection error:

Wherein K_l、P_l、L_l、M_lLocal map set, point feature set, line characteristic set, region feature set are respectively indicated, Ω is indicated The covariance matrix of incremental motion, point feature projection error e_ijIt is the two-dimensional distance that j-th of mapping point in the i-th frame observes, It calculates according to the following formula:

e_ij=x_ij-π(ζ_iw,X_wj),

π refers to the mapping from se (3) to (3) SE, ζ in formula_iwIt is the six-vector in se (3) space, X_wjIt is the three of point feature Dimension space coordinate, x_ijFor image coordinate；The projection error of its middle line feature and the projection error of point feature are different, by three-dimensional line segment Endpoint project to the plane of delineation, and regard it as error letter the distance between with Infinite Straight Line corresponding on the plane of delineation Number, e_ikIt is calculated by following formula:

P, Q are respectively the extreme coordinates of line feature, l in formula_ikThe linear equation in plane is represented, is provided by step 3；Region feature Projection error e_imIt is calculated according to the logarithmic mapping of quaternary number:

e_im=log { q [T (ζ_iw)^Tp(ζ_im)]^-1q(ζ_iw),

P (ζ in formula_im) indicate plane, it is provided by step 3, q (*) is a quaternary number, T (ζ_iw) it is the homogeneous of a transition matrix Form.It is iterated update using Levenberg-Marquardt optimization method, acquires optimal incremental motion.

5. a kind of vision SLAM method of multiple features fusion as described in claim 1, which is characterized in that offline building in advance Based on the bag of words of dotted line feature, successful match structural map Optimized model:

The description of BRIEF binary system is carried out to the line feature extracted, obtains description of line feature, the sub- structure of description with point feature It is matched at the vision entry vector of present frame according to the vision bag of words established offline in advance；If successful match, Two local maps where the two key frames need to be optimized with merging, and updated associated with the two local maps Local map is based on figure Optimization Framework, and using interframe incremental motion as the node of figure, the construction on side is similar to step 8, according to:

It is updated to be iterated, log therein and exp function respectively indicate Lie group to the logarithmic mapping and Lie algebra of Lie algebra To the index mapping of Lie group.