CN112862768B - Adaptive monocular VIO (visual image analysis) initialization method based on point-line characteristics - Google Patents

Abstract

The invention relates to an adaptive monocular VIO initialization method based on point and line features, which belongs to the technical field of robot vision positioning and navigation. Perform pre-integration calculation between image frames; S2: estimate the initial camera pose; S3: construct the maximum a posteriori estimation problem, optimize the inertial parameters, and obtain the scale factor, velocity information, gravity direction, and the gyroscope bias and accelerometer bias of the IMU position; S4: visual inertia alignment, scale scaling, and convert the initial camera pose to the world coordinate system; S5: initial value convergence. The invention can complete relatively stable and accurate initialization under different complex environments and different initial states, solves the uncertainty of the sensor and the inconsistency of inertial parameters in the VIO initialization process, and has higher performance.

Description

An Adaptive Monocular VIO Initialization Method Based on Point-Line Features

technical field

The invention belongs to the technical field of robot vision positioning and navigation, and relates to an adaptive monocular VIO initialization method based on point and line features.

Background technique

With the development of computer technology, research in the field of mobile robots has also developed rapidly. In order to realize the autonomous motion of the robot in an unknown environment, the first two problems to be solved are the real-time estimation of the robot pose and how to construct a map based on the pose, which further provides conditions for subsequent tasks such as autonomous positioning, path planning, and obstacle avoidance. In practical applications, robots are usually equipped with sensors with different functions. The SLAM system equipped with cameras and IMUs is called visual inertial SLAM, and its odometer is also called visual inertial odometry (VIO). It has a small size. , low cost, strong scene recognition ability and other advantages, has received extensive attention in this field.

For VIO, the initialization module is especially important. The determination of initial parameters, such as gravity direction, speed, IMU bias, etc., determines the accuracy of the system. Especially in the monocular VIO, the scale cannot be directly observed, it is difficult to integrate vision and inertia, and VIO initialization is difficult. For the initialization of the IMU, since the accelerometer of the IMU data is affected by gravity, the estimation of the direction of gravity is also a decisive factor in the pose estimation. If there is an error in the initialization work, the accuracy of the entire system will be degraded, and the optimization-based method may fall into a local optimum. The current initialization methods are mainly divided into tightly coupled and loosely coupled methods, and different solutions are proposed for the above problems. The document "Martinelli et al., Closed-formsolution of visual-inertial structure from motion. International Journal of Computer Vision, 2014" provides a closed solution scheme for jointly acquiring parameters such as scale, gravity, bias, and initial velocity to establish On the basis that the camera pose can be roughly estimated from the IMU data. Literature "Mur-Artal et al., Visual-inertial monocular SLAM with map reuse. IEEE Robotics and AutomationLetters, 2017" and literature "T. Qin et al., VINS-Mono: A robust and versatile monocularvisual-inertial state estimator. IEEE Transactions on Robotics, vol. 34, 2018" is based on the assumption that the monocular camera can accurately estimate the scale-free camera trajectory, and the inertial parameters are estimated from the camera trajectory and optimized by BA. The inertial parameters are solved by least squares in linear equations provided by visual information. However, the above two initialization schemes ignore the uncertainty of the sensor, and the inertia parameters are solved separately in different steps, ignoring their correlation.

To sum up, the current problems in the field of VIO technology are: 1) Too much reliance on scene features. In the existing VIO initialization algorithms, point features are generally used for pure visual estimation, but in weak texture environments, such as corridors, walls, etc., it is difficult to extract a sufficient number of feature points, resulting in initialization failure and poor system positioning accuracy. 2) The correlation between the uncertainty of the sensor and the inertial parameters is not considered, and the accelerometer bias of the IMU is usually ignored, resulting in a low accuracy of the estimated value. 3) The requirements for the initial state are high, and the camera needs to provide enough rotation and translation in the initialization stage to complete the initialization, which is only applicable to specific situations.

SUMMARY OF THE INVENTION

In view of this, the purpose of the present invention is that it is difficult to estimate the initial pose of the camera due to insufficient point features in a weak texture environment, the system positioning accuracy is poor, the sensor uncertainty and correlation are not considered in the inertial parameter estimation process, and the applicability of the initialization scheme is not suitable Strong and other problems, the line feature is introduced as an option in pure visual SFM, and it is used when the scene texture is not enough to provide reliable estimation to provide robustness; at the same time, a maximum a posteriori estimation problem is constructed to solve the inertial parameters to ensure their consistency. Applicable to any initialization situation, it provides an adaptive monocular VIO initialization method based on point and line features.

To achieve the above object, the present invention provides the following technical solutions:

An adaptive monocular VIO initialization method based on point and line features, comprising the following steps:

S1: Input image frames, detect point features and line features respectively; input data obtained by IMU, and perform IMU pre-integration calculation between each image frame;

S2: Estimate the initial pose of the camera; first determine whether the point features meet the parallax conditions and quantity requirements, if yes, solve the essential matrix by the eight-point method, and estimate the initial pose of the camera; otherwise, introduce the line feature, calculate its matching weak constraint score, filter Extract the line features for initialization, and estimate the initial pose of the camera according to the point-line distance constraints;

S3: Construct the maximum a posteriori estimation problem, optimize the inertial parameters, and obtain the scale factor, velocity information, gravity direction, and the gyroscope bias and accelerometer bias of the IMU;

S4: Visual inertial alignment, scale scaling, and convert the initial camera pose to the world coordinate system;

S5: The initial value converges, and the initialization is completed.

Further, step S1 specifically includes: detecting point features through the Shi-Tomasi corner point algorithm, which is detected based on gradient changes, and belongs to the improved algorithm of Harris corner point detection; line features adopt the LSD straight line detection algorithm, the core idea of which is to detect the gradient direction. Combine similar pixels to quickly detect straight line segments in the image; IMU pre-integration refers to integrating all IMU measurements between the kth frame and the k+1th frame of the image to obtain the PVQ between the k+1th frame. The values, i.e. the values of position, velocity and rotation, provide initial values for vision and serve as constraints for backend optimization.

Further, in step S2, the initial pose of the camera is estimated in two cases according to whether the point feature satisfies the initialization conditions:

Case 1: The point feature meets the parallax condition and quantity requirements, and the relationship between the corresponding points obtained according to the epipolar geometry is:

where x ₁ =(u ₁ ,v ₁ ,1) ^T , x ₂ =(u ₂ ,v ₂ ,1) ^T is the coordinate on the normalized plane of the corresponding pixel, R and t are the camera between the two frames Motion, representing rotation and translation respectively; the middle part is denoted as the essential matrix E, represented as E=t^R, which is a 3×3 matrix with 5 degrees of freedom;

The essential matrix is solved by the eight-point method, which is obtained from the epipolar geometry:

In order to solve E, a total of eight pairs of matching points are required to form eight equations, and the essential matrix E is solved by singular value decomposition (SVD), and the solution with positive depth is taken as the final estimate;

Case 2: The point feature does not meet the initialization requirements, the line feature is introduced, the matching pairs are selected by calculating the weak constraint score, and the initial camera pose is solved by the point-line distance constraint; among them, the weak constraints include descriptor constraints and epipolar constraints, respectively It calculates the scores s _d and s _e as follows:

The LSD line segment adopts the LBD descriptor, and calculates the pixel gradient and calculates the average vector and standard deviation of the statistic as the descriptor; for the descriptor constraint, the reference frame descriptor desc The Hamming distance between ₁ and the current frame descriptor desc ₂ , if it is less than the threshold τ _desc , the descriptor score s _d is recorded as 1, if it is greater than the threshold, the descriptor score s _d is recorded as 0, expressed as:

For the epipolar constraint, since the line feature does not have strict epipolar constraint, it is used as a weak constraint to enhance reliability; first, the epipolar line of the two endpoints of the reference frame line feature is calculated, and the line where the corresponding line feature AB of the current frame is located is the same as the epipolar line. Intersecting at points C and D, the constraint score is defined as:

where d _min represents the minimum Euclidean distance of four collinear points, and d _max represents the maximum Euclidean distance of four collinear points;

Finally, for each pair of matching lines, calculate the score s=s _d · s _e , if s is greater than a certain threshold, it is considered that the matching pair can be used for initialization, and closed-form solution is performed;

The closed-form solution process is as follows: the endpoint projection of the 3D line feature should theoretically fall on the line observed by the camera, and the coefficients of the normalized line feature are obtained;

The inverse depths of the endpoints of the line feature are ρ _ks and ρ _ke respectively, then the reprojection of the 3D line endpoints can be expressed as:

where π(·) is the reprojection function, expressed as π(x,y,z) ^T =π(x/z,y/z,1) ^T , and R _i is the rotation matrix based on the small rotation assumption, that is, the assumption The rotation between consecutive image frames is small, and the camera rotation vector and translation vector are respectively r=(r ₁ , r ₂ , r ₃ ) ^T and t=(t ₁ , t ₂ , t ₃ ) ^T , the rotation matrix is The first-order Taylor expansion is approximated by:

即：Since the projection point is on the observation line, the distance between the two is zero; taking the starting point as an example, the constraint is expressed as

which is:

Under the small rotation assumption, ρ _ks t ₁ is negligible, so the above formula simplifies to:

Ar ₁ +Br ₂ +Cr ₃ +D=0

in:

也具有相同约束，因此一对匹配线得到两个方程；如果有多对匹配线，则通过如下线性方程闭式求解，由SVD得到唯一解；Also, another endpoint

It also has the same constraints, so a pair of matching lines obtains two equations; if there are multiple pairs of matching lines, the following linear equations are solved in closed form, and the unique solution is obtained by SVD;

Further, in step S3, a maximum a posteriori estimation problem is constructed, and the relevant parameters of the IMU are optimized to obtain the scale factor, velocity information, gravity direction, and the gyroscope bias and accelerometer bias of the IMU;

First, the estimated inertial parameters are:

是无尺度的第0帧到第k帧的速度；由IMU预积分理论建立含先验的MAP问题；where s is the scale factor, R _wg is the direction of gravity, and the b vector includes the IMU accelerometer bias b _a and the gyroscope bias b _g ,

is the speed from the 0th frame to the kth frame without scale; the MAP problem with prior is established by the IMU pre-integration theory;

似然值，

是先验值，

表示初始化窗口内连续关键帧之间 IMU预积分的集合；假设IMU每次的测量值是独立的，该MAP问题描述为：of which is

Likelihood,

is the prior value,

Represents the set of IMU pre-integrations between consecutive keyframes within the initialization window; assuming that each measurement of the IMU is independent, the MAP problem is described as:

Assuming that the errors of the IMU pre-integration and prior distribution are Gaussian errors, the final optimization problem is obtained:

为IMU预积分误差；且在优化过程中，重力方向和尺度因子的更新式为：where r _p is the prior error,

is the IMU pre-integration error; and in the optimization process, the update formulas of the gravity direction and scale factor are:

s ^new =s ^old exp(δ _s )

The method considers the uncertainty of the IMU, establishes the estimation of inertial parameters as an optimal estimation problem, and does not need to assume that the bias of the accelerometer is ignored, and adds the known information to the MAP problem as a priori information; one-time estimation All inertial parameters are obtained to avoid the problem of data inconsistency.

Further, in step S4, after the optimization of the inertial parameters is completed, the estimated value of the scale information required for monocular vision is obtained, and the scale is scaled according to the scale to obtain the camera pose, speed and 3D map point, and align with the direction of gravity, Convert the pose to the world coordinate system, recalculate the IMU pre-integration and update it; so far, the visual and inertial parameters have been estimated separately, and finally BA optimization is performed to obtain the optimal solution.

The beneficial effects of the present invention are: 1) the present invention improves the problems of low precision, poor robustness, and insufficient applicability of traditional methods, and can complete relatively stable and accurate initialization under different complex environments and different initial states; 2) this The adaptive pure visual SFM estimation method of the introduced line feature proposed by the invention can well adapt to the weak texture environment, and provides structural information to improve reliability; 3) The inertial optimization method based on maximum a posteriori estimation proposed by the invention can be well The uncertainty of the sensor and the inconsistency of the inertial parameters during the initialization of the VIO can be effectively solved. The simulation results show that the present invention has higher performance than the existing VIO algorithm.

Other advantages, objects, and features of the present invention will be set forth in the description that follows, and will be apparent to those skilled in the art based on a study of the following, to the extent that is taught in the practice of the present invention. The objectives and other advantages of the present invention may be realized and attained by the following description.

Description of drawings

In order to make the objectives, technical solutions and advantages of the present invention clearer, the present invention will be preferably described in detail below with reference to the accompanying drawings, wherein:

1 is a flowchart of an adaptive monocular VIO initialization algorithm based on dotted line features provided by an embodiment of the present invention;

2 is a flowchart of line feature processing provided by an embodiment of the present invention;

3 is a schematic diagram of a line feature-to-pole constraint provided by an embodiment of the present invention;

4 is a schematic diagram of feature extraction in a weak texture environment provided by an embodiment of the present invention;

Fig. 5 is the contrast schematic diagram between the trajectory that the method adopted in the present invention and the traditional VIO method obtain and the real trajectory;

FIG. 6 is a schematic diagram showing the comparison of the root mean square error (RMSE) obtained by the VIO method adopted in the present invention and the traditional VIO method.

Detailed ways

The embodiments of the present invention are described below through specific specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and various details in this specification can also be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the drawings provided in the following embodiments are only used to illustrate the basic idea of the present invention in a schematic manner, and the following embodiments and features in the embodiments can be combined with each other without conflict.

Among them, the accompanying drawings are only used for exemplary description, and represent only schematic diagrams, not physical drawings, and should not be construed as limitations of the present invention; in order to better illustrate the embodiments of the present invention, some parts of the accompanying drawings will be omitted, The enlargement or reduction does not represent the size of the actual product; it is understandable to those skilled in the art that some well-known structures and their descriptions in the accompanying drawings may be omitted.

The same or similar numbers in the drawings of the embodiments of the present invention correspond to the same or similar components; in the description of the present invention, it should be understood that if there are terms “upper”, “lower”, “left” and “right” , "front", "rear" and other indicated orientations or positional relationships are based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the indicated device or element must be It has a specific orientation, is constructed and operated in a specific orientation, so the terms describing the positional relationship in the accompanying drawings are only used for exemplary illustration, and should not be construed as a limitation of the present invention. situation to understand the specific meaning of the above terms.

Please refer to FIG. 1 to FIG. 6 , which are an adaptive monocular VIO initialization method based on point-line features.

1 is a flowchart of an adaptive monocular VIO initialization algorithm based on dotted line features provided by an embodiment of the present invention. As shown in the figure, the adaptive monocular VIO initialization algorithm based on dotted line features provided by an embodiment of the present invention includes:

First, point features are detected by the Shi-Tomasi corner point algorithm, which is based on gradient changes and belongs to the improved algorithm of Harris corner point detection. The line feature adopts the LSD line detection algorithm, and its core idea is to combine pixels with similar gradient directions to quickly detect straight line segments in the image. IMU pre-integration refers to integrating all the IMUs between the kth frame and the k+1th frame of the image, then the PVQ value between the k+1th frame, that is, the position, velocity and rotation values, can be obtained for vision. The initial value is used as a constraint for backend optimization.

Then, according to whether the point features meet the initialization conditions, the initial pose of the camera is estimated in two cases:

Case 1: The point feature meets the parallax condition and quantity requirements. According to the epipolar geometry, the relationship between the corresponding points can be obtained as:

where x ₁ =(u ₁ ,v ₁ ,1) ^T , x ₂ =(u ₂ ,v ₂ ,1) ^T is the coordinate on the normalized plane of the corresponding pixel, R and t are the camera between the two frames Motion, representing rotation and translation, respectively. The middle part is denoted as the essential matrix E, expressed as E=t^R, which is a 3×3 matrix with 5 degrees of freedom.

The essential matrix is solved by the eight-point method, which can be obtained from the epipolar geometry:

To solve E, a total of eight pairs of matching points are required to form eight equations, the essential matrix E is solved by singular value decomposition (SVD), and the solution with positive depth is taken as the final estimate.

Case 2: The point feature does not meet the initialization conditions, the line feature is introduced, the matching pairs are selected by calculating the weak constraint score, and the initial camera pose is solved by the point-line distance constraint. Among them, weak constraints include descriptor constraints and epipolar constraints, and the scores s _d and s _e are calculated respectively for them. The process is shown in Figure 2 . The process is as follows:

The LSD line segment adopts the LBD descriptor, and the pixel gradients are counted and the average vector and standard deviation of the statistics are calculated as the descriptor. For the descriptor constraints, the Hamming distance between the reference frame descriptor desc ₁ and the current frame descriptor desc ₂ is calculated mainly considering the need to eliminate the mismatches with large appearance differences. If it is less than the threshold τ _desc , the descriptor score s _d It is recorded as 1. If it is greater than the threshold, the descriptor score s _d is recorded as 0, which is expressed as:

For epipolar constraints, since the line features do not have strict epipolar constraints, they are used as weak constraints to enhance reliability. As shown in Figure 3, the epipolar constraint of the two ends of the line segment is represented. First, the epipolar lines l ₁ and l ₂ of the two end points of the reference frame line feature are calculated. The line corresponding to the line feature AB of the current frame intersects the epipolar line at the point C and point D. The constraint score is defined as:

Where d _min represents the minimum Euclidean distance of four collinear points, and d _max represents the maximum Euclidean distance of four collinear points.

Finally, for each pair of matching lines, a score s=s _d ·s _e is calculated, and if s is greater than a certain threshold, the matching pair is considered to be available for initialization and closed-form solution is performed. The closed-form solution process is as follows: The projection of the endpoints of the 3D line feature should theoretically fall on the line observed by the camera, so the coefficients of the normalized line feature can be obtained:

The inverse depths of the endpoints of the line feature are ρ _ks and ρ _ke respectively, then the reprojection of the 3D line endpoints can be expressed as:

where π(·) is the reprojection function, which can be expressed as π(x,y,z) ^T =π(x/z,y/z,1) ^T , R _i is the rotation matrix based on the small rotation assumption, that is Assuming that the rotation between consecutive image frames is small, record the camera rotation vector and translation vector as r=(r ₁ , r ₂ , r ₃ ) ^T and t=(t ₁ , t ₂ , t ₃ ) ^T , respectively, the rotation matrix It can be approximated by the first-order Taylor expansion:

即：Since the projection point is on the observation line, the distance between the two is zero. Taking the starting point as an example, the constraint can be expressed as

which is:

Under the small rotation assumption, ρ _ks t ₁ is negligible, so the above formula simplifies to:

Ar ₁ +Br ₂ +Cr ₃ +D=0

in:

也具有相同约束。因此一对匹配线可得到两个方程。如果有多对匹配线，则通过如下线性方程闭式求解，由SVD得到唯一解：Also, another endpoint

also has the same constraints. So a pair of matched lines results in two equations. If there are multiple pairs of matching lines, the following linear equation is solved in closed form, and the unique solution is obtained by SVD:

For inertial estimation, the maximum a posteriori estimation problem is constructed, and the relevant parameters of the IMU are optimized to obtain the scale factor, velocity information, gravity direction, and the gyroscope bias and accelerometer bias of the IMU. First, the estimated inertial parameters are:

是无尺度的第0帧到第k帧的速度。由IMU预积分理论可以建立含先验的MAP问题：where s is the scale factor, R _wg is the direction of gravity, and the b vector includes the IMU accelerometer bias b _a and the gyroscope bias b _g ,

is the speed of unscaled frames 0 to k. The MAP problem with priors can be established from the IMU pre-integration theory:

似然值，

是先验值，

表示初始化窗口内连续关键帧之间IMU预积分的集合。假设IMU每次的测量值是独立的，该MAP问题可以描述为：of which is

Likelihood,

is the prior value,

Represents the set of IMU preintegrations between consecutive keyframes within the initialization window. Assuming that each measurement of the IMU is independent, the MAP problem can be described as:

Assuming that the errors of the IMU pre-integration and prior distribution are Gaussian errors, the final optimization problem can be obtained:

为IMU预积分误差。且在优化过程中，重力方向和尺度因子的更新式为：where r _p is the prior error,

is the IMU pre-integration error. And in the optimization process, the update formula of gravity direction and scale factor is:

s ^new =s ^old exp(δ _s )

The method considers the uncertainty of the IMU, establishes the estimation of inertial parameters as an optimal estimation problem, and does not need to assume that the bias of the accelerometer is ignored, and adds the known information as a priori information to the MAP problem. All inertial parameters can be estimated at one time, avoiding the problem of data inconsistency.

After the optimization of inertial parameters is completed, the estimated value of scale information required for monocular vision can be obtained. By scaling according to the scale, the camera pose, speed and 3D map points can be obtained, and aligned with the direction of gravity to convert the pose to In the world coordinate system, recalculate and update the IMU pre-integration. So far, the visual and inertial parameters have been estimated respectively, and finally the BA optimization is carried out to obtain the optimal solution.

FIG. 4 is a schematic diagram of feature extraction in a weak texture environment provided by an embodiment of the present invention. As can be seen from the figure, when the environment texture is not obvious, it is difficult to extract point features. At this time, introducing line features as structural information can solve this problem. , which enhances the robustness of VIO.

The present invention conducts experiments using the mainstream dataset Euroc. The data set uses micro air vehicles (MAV) to collect image information and IMU information in industrial environments, and contains 11 sequences in total, which are divided into three types: simple, medium and difficult according to lighting conditions, textures and motion speeds, and are suitable for testing the invention. performance.

5 is a schematic diagram of the comparison between the trajectory obtained by the VIO method adopted by the present invention and the real trajectory obtained by the traditional VIO method, wherein (a) is a schematic diagram of the trajectory of the V2_01_easy sequence, the sequence has insufficient parallax in the initial stage, and the translation is small; (b) ) is a schematic diagram of the trajectory of the MH_05_difficult sequence, which remains almost stationary at the initial stage and in a light-less, texture-less environment for a long time. It can be seen that the trajectory obtained by the VIO method adopted in the present invention is closer to the real value, thereby verifying that the method of the present invention has better accuracy.

6 is a schematic diagram of the root mean square error (RMSE) comparison between the VIO method adopted by the present invention and the traditional VIO method, wherein (a) is the root mean square error (RMSE) variation of the V2_01_easy sequence, and (b) is the mean square error (RMSE) of the MH_05_difficult sequence. Root square error (RMSE) variation. It can be seen that the root mean square error (RMSE) value obtained by the VIO method adopted in the present invention is generally lower than that obtained by the traditional VIO method, and the variation range is also small, thus verifying that the method of the present invention has better stability.

Table 1 summarizes the translation error (Translation) and the rotation error (Rotation) of the VIO method of the present invention and the traditional VIO algorithm under the Euroc data set, and both use the root mean square error (RMSE). It can be seen from the data in Table 1 that the VIO method adopted in the present invention can achieve better results.

The monocular VIO initialization algorithm based on point and line features proposed by the present invention effectively solves the problems of low precision, poor robustness and insufficient applicability of traditional methods, and can be completed stably and accurately in different complex environments and different initial states. The initialization of , introduces line features, so that the initialization can adaptively perform pure visual estimation according to environmental changes, which enhances the reliability. In addition, the uncertainty of the sensor and the inconsistency of inertial parameters in the VIO initialization process are well resolved; the simulation results show that the invention has certain improvements in initialization real-time, accuracy and stability, and has good performance. Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to the preferred embodiments, those of ordinary skill in the art should understand that the technical solutions of the present invention can be Modifications or equivalent replacements, without departing from the spirit and scope of the technical solution, should all be included in the scope of the claims of the present invention.

Claims (4)

1. A self-adaptive monocular VIO initialization method based on point-line characteristics is characterized in that: the method comprises the following steps:

s1: inputting an image frame, and respectively detecting a point feature and a line feature; inputting data acquired by the IMU, and performing IMU pre-integration calculation between each image frame;

s2: estimating an initial pose of the camera; firstly, judging whether the point characteristics meet the requirements of parallax error conditions and quantity, if so, solving an essential matrix by an eight-point method, and estimating the initial pose of a camera; otherwise, introducing the line features, calculating the matched weak constraint scores, screening out the line features for initialization, and estimating the initial pose of the camera according to the point-line distance constraint; estimating the initial pose of the camera according to two conditions that whether the point characteristics meet the initialization conditions:

case 1: the point characteristics meet the requirements of parallax error conditions and quantity, and the relation of corresponding points obtained according to epipolar geometry is as follows:

wherein x ₁ ＝(u ₁ ,v ₁ ,1) ^T 、x ₂ ＝(u ₂ ,v ₂ ,1) ^T Is the coordinate on the normalization plane of the corresponding pixel point, and R and t are the camera motion between two frames, representing rotation and translation, respectively; the middle part is marked as an intrinsic matrix E, expressed as E ^ t ^ R, and is a 3 multiplied by 3 matrix with 5 degrees of freedom;

the essential matrix is solved by an eight-point method, which is obtained from the epipolar geometry:

in order to solve the E, eight equations are formed by eight pairs of matching points, an essential matrix E is solved by singular value decomposition SVD, and a solution with positive depth is taken as a final estimation;

case 2: introducing line features when the point features do not meet the initialization requirement, screening matched pairs by calculating weak constraint scores, and solving the initial pose of the camera by point-line distance constraint; wherein the weak constraint comprises a descriptor constraint and an epipolar constraint, and the fraction s is calculated for the descriptor constraint and the epipolar constraint respectively _d And s _e (ii) a The case 2 specifically includes the following steps:

s221: the LSD line segment adopts an LBD descriptor, the pixel gradient is counted, and the average vector and the standard variance of the statistic are calculated to be used as the descriptor; for descriptor constraint, mainly considering that mismatching with larger appearance difference needs to be eliminated, calculating reference frame descriptor desc ₁ And a current frame descriptor desc ₂ Hamming distance therebetween, if less than the threshold τ _desc Descriptor score s _d Marking as 1, if the value is larger than the threshold value, describing a sub-score s _d Noted as 0, expressed as:

s222: the epipolar constraint is used as a weak constraint item to enhance the reliability; firstly, calculating epipolar lines of two end points of the line characteristics of a reference frame, wherein a straight line where the corresponding line characteristics AB of the current frame are located intersects the epipolar lines at a point C and a point D, and the epipolar constraint fraction is defined as:

wherein d is _min Representing the minimum Euclidean distance of four collinear points, d _max Represents the maximum Euclidean distance of the four collinear points;

s223: match for each pairLine, calculating the fraction s ═ s _d ·s _e If s is larger than a preset threshold value, the match line pair is considered to be available for initialization, and closed-type solution is carried out;

s3: constructing a maximum posterior estimation problem, optimizing inertial parameters, and obtaining a scale factor, speed information, a gravity direction, and gyroscope bias and accelerometer bias of the IMU; step S3 specifically includes: constructing a maximum posterior estimation problem, and optimizing relevant parameters of the IMU to obtain a scale factor, speed information, a gravity direction, and gyroscope bias and accelerometer bias of the IMU;

first, the estimated inertial parameters are:

where s is a scale factor, R _wg For gravity direction, the b vector includes IMU accelerometer bias b _a And gyroscope bias b _g ，

Is the speed of the 0 th frame to the k th frame of no scale; establishing a priori-contained MAP problem by IMU pre-integration theory:

wherein

Is the value of the likelihood that,

is a value that is a priori known to the user,

representing a set of IMU pre-integrals between successive keyframes within an initialization window; each measurement by the IMU is independent, and the MAP problem is described as：

The error of IMU pre-integration and prior distribution is Gaussian error, and the final optimization problem is as follows:

wherein r is _p In order to be a priori the error,

pre-integrating the error for the IMU; and in the optimization process, the updating formula of the gravity direction and the scale factor is as follows:

s ^new ＝s ^old exp(δ _s )

s4: visual inertia alignment, scaling and simultaneously converting the initial pose of the camera into a world coordinate system;

s5: the initial value converges and the initialization is completed.

2. The dotted line feature-based adaptive monocular VIO initialization method of claim 1, wherein: step S1 specifically includes: detecting point characteristics through a Shi-Tomasi corner algorithm; line characteristics adopt an LSD (least squares distortion) linear detection algorithm, pixels with similar gradient directions are combined, and linear segments in an image are rapidly detected; the IMU pre-integration means that all IMU measurement values between the kth frame and the (k + 1) th frame of an image are integrated to obtain PVQ values between the (k + 1) th frame, namely position, speed and rotation values, initial values are provided for vision, and the initial values are used as constraint terms of back-end optimization.

3. The dotted line feature-based adaptive monocular VIO initialization method of claim 1, wherein: the closed solving process in step S223 is as follows:

the end projection of the 3D line feature theoretically falls on the line observed by the camera, obtaining the coefficients of the normalized line feature:

the inverse depths of the end points of the line marking characteristic are respectively rho _ks And ρ _ke Then the 3D line end reprojection is normalized to be:

where π (. cndot.) is the reprojection function, expressed as π (x, y, z) ^T ＝π(x/z,y/z,1) ^T ，R _i Based on a rotation matrix under the assumption of small rotation, that is, assuming that rotation between successive image frames is small, let r be (r) for a camera rotation vector and a translational vector, respectively ₁ ,r ₂ ,r ₃ ) ^T And t ═ t (t) ₁ ,t ₂ ,t ₃ ) ^T The rotation matrix is approximately expressed as a first order Taylor expansion:

the distance between the projection point and the observation line is zero; taking the starting point as an example, the constraint is expressed as

Namely:

under the assumption of a small rotation, the rotation speed of the rotor,ρ _ks t ₁ neglected, so the above equation is simplified:

Ar ₁ +Br ₂ +Cr ₃ +D＝0

wherein:

in addition, the other end point

Also have the same constraints, so a pair of match lines yields two equations; if there are multiple pairs of match lines, then the unique solution is obtained from SVD by solving the following linear equation closed form:

4. the dotted line feature-based adaptive monocular VIO initialization method of claim 1, wherein: step S4 specifically includes: after the inertial parameters are optimized, obtaining a scale information estimation value required by monocular vision, carrying out scaling according to the scale to obtain a camera pose, a speed and a 3D map point, aligning the camera pose, the speed and the 3D map point with the gravity direction, converting the pose into a world coordinate system, and recalculating IMU pre-integration and updating; and finally, performing BA optimization to obtain an optimal solution.

Images