Line feature pose-only measurement model construction method suitable for visual SLAM (Simultaneous Localization and Mapping)

Abstract

The invention discloses a line feature pose-only measurement model construction method suitable for visual SLAM (Simultaneous Localization and Mapping), and belongs to the field of SLAM. According to a traditional SLAM line feature, when a reprojection error is constructed, the line feature needs to be triangularized, the introduced triangularized error enables the positioning precision to be reduced, and state variables are increased along with the increase of the line feature, and in view of the problems, when the line feature extracted by the visual front end is used for constructing the pose-only reprojection error, the positioning precision is greatly improved. The method comprises the following steps: constructing a reprojection error of a line feature in a current frame by utilizing three-frame constraint, giving an error state Jacobian solving method of the corresponding line feature reprojection error relative to a state variable, and further providing a line feature screening and only pose base frame selection mode so as to improve the positioning precision. When the method is fused into a point-only pose system for use, the positioning precision can be effectively improved, the number of state variables is reduced, and when the method is applied to an MSCKF framework, the current frame state of the system can be corrected in real time.

Description

Technical Field

[0001] The invention belongs to the field of SLAM and is a line feature back-end pose estimation method suitable for visual SLAM. Background Art

[0002] In the field of robotics, simultaneous localization and mapping (SLAM) technology has always attracted the attention of scholars, and visual SLAM technology based on point features has become increasingly mature. However, based only on point features, the problem of reduced positioning accuracy caused by sparse feature points is exposed in weak texture scenes. To solve this problem, scholars introduced other structural features, such as classic line features. For line features, the current mainstream method is similar to the processing of point features. The visual front end extracts line features, reprojects the line features after triangulation, and constructs constraints using the reprojection error. However, this inevitably introduces the triangulation error of the line features. No matter what framework the back end is based on, it cannot effectively correct the impact of the triangulation error. Summary of the invention

[0003] Traditional SLAM line features need to be triangulated when constructing reprojection errors, which will introduce triangulation errors and reduce positioning accuracy. In addition, with the increase of line features, the state variables increase accordingly. In response to these problems, the present invention proposes a line feature only pose measurement model construction method suitable for visual SLAM based on the point feature only pose method, which is integrated into the point feature only pose VINS (Visual-Inertial Navigation System, Visual Inertial Navigation System), and directly constructs line reprojection through three-frame observation, so that the reprojection error variables only contain the state variables of the system, and there is no need to triangulate the line features to calculate the coordinates of the line features in the local world system, thereby effectively improving the positioning accuracy.

[0004] A method for constructing a line feature pose measurement model suitable for visual SLAM of the present invention comprises the following steps:

[0005] Step 1: Use the line features extracted by the visual front end to construct the pose reprojection error. Use three frames of common view frames to construct the reprojection error of the line feature l in the current frame. The three frames of common view frames include the frame C where the line feature l is first observed. i , current frame C k And in C i and C k The observation frame C of a common view l between j ; The construction method of the reprojection error of the line feature l in the current frame includes:

[0006] First, through frame Ci and C j Get line feature l in C i The distance parameter from the camera frame to the camera optical center Reuse frame C i and C k The pose relationship between them obtains the line feature l in the current frame C k The prediction result of the reprojected straight line under , without the need to triangulate the line feature, is to obtain the homogeneous coordinates of the straight line Further calculate the reprojection error e of the line feature l in the current frame l ;

[0007] in,

[0008] Intermediate variables Among them, the line feature l is obtained in frame C i , C j The unit normal vector of the back-projection plane formed by the camera optical center and the two endpoints of the normalized plane of the line feature l under the camera C j Camera to C i The rotation matrix of the camera system, Indicates frame C j To frame C i The translation vector of ; sgn is the sign function; variable and For frame C i The coordinates of the two endpoints of the line feature l in the dedistorted pixel plane. The superscript T indicates transposition.

[0009] Step 2: Calculate the error state Jacobian of the line feature reprojection error with respect to the state variables to correct the state variables. Use the chain derivation method to calculate the error state Jacobian matrix of the reprojection error with respect to the camera pose, as well as the Jacobian matrix of the reprojection error with respect to the camera extrinsic parameters and intrinsic parameters, which are used to correct the pose of the inertial sensor IMU, the camera extrinsic parameters and the camera intrinsic parameters.

[0010] Step 3: On the basis of line feature reprojection, set up line feature screening and pose-based frame selection. j Filtering; Among them, the common view frame is selected according to the parallax angle, and the intermediate common view frame C of the line feature j , to obtain the target with the largest parallax to select; for line features, (1) if there is an intermediate common view frame and the first observation frame C i If the parallax angle between the two frames is less than the threshold, the line feature is removed. (2) Calculate the line feature of each intermediate common view frame in C iThe distance parameter from the camera system to the camera optical center is removed. The unreasonable intermediate frames whose distance parameter is less than 0 are removed. The coefficient of variation is calculated according to the mean and variance of the distance parameter of the reasonable intermediate frames. The line features whose coefficient of variation is not less than the set threshold are removed.

[0011] Compared with the prior art, the advantages and positive effects of the present invention are:

[0012] (1) The method of the present invention does not need to triangulate the line features when constructing the reprojection error, which avoids the problem of introducing line feature triangulation errors. At the same time, it also does not need to construct a 4-DOF representation of the line features, making the introduction method of the line features more intuitive and concise.

[0013] (2) The method of the present invention does not increase the state variables when the line features are introduced, and there is no need to use the line feature parameters as state variables. It can be seen from the experiment that the positioning error of visual SLAM can be effectively corrected by using the method of the present invention.

[0014] (3) When the method of the present invention is applied in the MSCKF framework, it can realize real-time correction of the current frame state of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 It is a schematic diagram of the overall process of the method for constructing a line feature only posture measurement model according to an embodiment of the present invention;

[0016] Figure 2 It is a schematic diagram of a straight line in space;

[0017] Figure 3 It is a three-frame common view schematic diagram used when constructing the reprojection error of line features by the method of the present invention;

[0018] Figure 4 It is a trajectory evaluation diagram of an embodiment of the present invention applying the method of the present invention and the comparative method under the MH-05 sequence;

[0019] Figure 5 is a trajectory APE error diagram of an embodiment of the present invention applying the method of the present invention and the comparative method under the MH-05 sequence;

[0020] Figure 6 It is a roll angle estimation curve diagram of the method of the present invention and the comparative method under the V2-03 sequence according to the embodiment of the present invention. DETAILED DESCRIPTION

[0021] The present invention will be further described in detail below with reference to the accompanying drawings.

[0022] The method for constructing a line feature pose-only measurement model suitable for visual SLAM according to an embodiment of the present invention can be implemented as a functional module by a computer and integrated into VINS. In an embodiment of the present invention, a reprojection error construction module, an error update module, and a line feature and common view frame screening module are provided. The three modules can be implemented as computer functional software modules, or implemented in hardware or dedicated circuits, software, firmware, logic, or any combination thereof. The following describes the construction and error update of the pose reprojection error of line features, as well as the screening of line features and common view frames in three steps. An implementation process of an embodiment of the present invention is as follows: Figure 1 As shown, when using the method of the present invention, line features and common view frames are first screened and then reprojection errors are calculated.

[0023] Step 1: The reprojection error construction module constructs the pose-only reprojection error using the line features extracted by the vision front end.

[0024] In visual odometer, it is necessary to construct feature constraints to solve the camera pose, and the same is true for the introduction of line features. Before this, it is clear that the straight line description method used in the present invention is to choose Plücker coordinate representation. Figure 2 As shown, where n e Represents the normal vector of the line back-projection plane formed by the optical center O of the current camera frame and the two endpoints of the line, v e Indicates the direction vector of the camera system in the current frame. The subscript e indicates that the vector is a unit vector. l Indicates the distance from the spatial straight line to the optical center of the camera in the current frame. Figure 2 The O-xyz in the figure is the camera system of the current frame, and the z axis is the camera optical axis.

[0025] When the two endpoints of the line feature l in three-dimensional space have coordinates p f1 、p f2 When it is known, the Plücker coordinate representation of the line can be obtained:

[0026]

[0027] In the formula, [] × Indicates the antisymmetric matrix, n l 、v l They are the normal vector and direction vector of the back-projection plane corresponding to the line feature l in the current frame, and the distance from the origin of the coordinate system to the straight line is:

[0028]

[0029] After the description method is clarified, the reprojection error of the line feature is constructed. Different from the classical method with multi-frame constraints, the reprojection error construction method proposed in the present invention requires the selection of three co-viewing frames, and does not require triangulation to obtain the coordinate representation of the line feature in the world system. The reprojection error is constructed only by using the inter-frame constraints. Figure 2 To illustrate the specific implementation method.

[0030] like Figure 2 As shown, select image frame C i , C j and C k Only the pose reprojection error is used to construct the line, where the kth frame C k Indicates the current frame, the i-th frame C i is the image frame where the line feature is first observed, the jth frame C j For a common viewing observation frame between the two frames, the following steps 1.1 to 1.6 are then performed.

[0031] Step 1.1: Get line features in image frame C i The direction vector in the camera frame.

[0032] The core of constructing the reprojection error is to use historical observations to predict the coordinates of the line features in the current frame. The present invention achieves this goal through clever equation construction.

[0033] Figure 2 As shown, the unit normal vector of the back-projection plane formed by the camera optical center and the two endpoints of the line feature normalization plane of the line feature l in the i-th, j-th, and k-th frame camera system is obtained respectively. and Will and All converted to C i The camera is in the same frame. At this point, the two normal vectors obtained by transformation are and and line feature l in C i Direction vector in the camera frame Vertical, calculated from as follows:

[0034]

[0035] In the formula, sgn represents the sign function, and the variable ζ is calculated according to formula (1.4), is the rotation matrix from the jth frame to the ith frame.

[0036] In order to satisfy the direction constraints of the starting and ending points of the line segment, that is, to have a unified direction, the variable ζ is defined as follows:

[0037]

[0038] In the formula, and For image frame C i The two endpoint coordinates of the line feature in the dedistorted pixel plane. The superscript T indicates transposition.

[0039] Step 1.2: Calculate the line feature l in C i The distance from the camera to the camera optical center

[0040] According to the relationship between line features in two image frames, we can get:

[0041]

[0042] In the formula, represents the translation vector from the jth frame to the ith frame, Indicates that the line feature l is in C i The distance from the camera frame to the camera optical center.

[0043] because The denominator term in the equation makes the Jacobian solution more complicated. To reduce the state variables in the denominator, we define an intermediate variable

[0044]

[0045] In the formula, l h v represents the direction vector function, the subscript G represents the world coordinate system, Represents the world system to C i Camera, C j The rotation matrix of the camera system. At this point we can get:

[0046]

[0047] In the formula, is defined as:

[0048]

[0049] Taking the two norms on both sides of equation (1.7) to get its specific value:

[0050]

[0051] In the formula, l h d It represents the distance function, In the world system C i The pose of the camera system, Indicates that in the world system C j The pose of the camera system.

[0052] Step 1.3: Predict line features in C k Normal vector in camera system

[0053] Similar to the above coordinate transformation, through the pose relationship between frames i and k, the normal vector estimate of the line feature in the kth frame can be obtained:

[0054]

[0055] In the formula, represents the rotation matrix from the i-th frame to the k-th frame, represents the translation vector from the i-th frame to the k-th frame, l h tk represents the normal vector estimation function, C i The Through the above steps, it has been calculated successively, so it can be obtained that the line feature is in C k Normal vector in camera system The prediction results as line features.

[0056] Step 1.4: The predicted line feature normal vector Projection to the pixel plane.

[0057] After completing the prediction of line features, C k The predicted straight line of the camera system is projected onto the pixel plane to obtain the homogeneous coordinates of the straight line

[0058]

[0059] In the formula, K l is the projection matrix of the line feature, l h p represents the projection function, f x 、f y are the focal lengths of the camera on the x-axis and y-axis, respectively, x 、c y They are the horizontal and vertical positions of the camera optical center in the pixel coordinate system.

[0060] Step 1.5: Calculate the reprojection error.

[0061] The error of the reprojected line is measured by the distance between the observed line endpoint and the reprojected line. Therefore, the reprojection error e of the current line feature in the kth frame is l It can be expressed as:

[0062]

[0063] In the formula, and are the homogeneous coordinates of the two endpoints of the observation line feature in the pixel plane, that is, l h I represents the error function of the line, Indicates that in the world system C k The pose of the camera system, including translation With the rotation matrix

[0064] In the calculation expression of the line feature reprojection error, e l It does not contain the parameters related to line feature calculation. It is only a function of the pose and camera internal and external parameters and will not increase with the increase of features.

[0065] Line feature reprojection error and The modulus value of Therefore, in formula (1.10), the intermediate variable constructed by the method of the present invention is and It is reasonable.

[0066] Step 1.6: Adaptive adjustment of observation covariance.

[0067] The embodiment of the present invention is implemented by extending the Kalman filtering method. From time k-1 to time k, there is a system state transfer equation and a state observation equation. The state observation equation is: Z k =Hx k +v k ; Among them, Z k is the observed quantity, H is the observation matrix, x k is the state variable, here is the state of the camera, v k To observe the noise, set it to obey the zero-mean Gaussian distribution: k ~N(0,Q).

[0068] In order to prevent the line reprojection error from being too large and affecting the estimation results, the Huber kernel function is used to adaptively adjust the observation noise covariance. The specific method is as follows:

[0069] Huber kernel function ρ(e l,r )for:

[0070]

[0071] In the formula, e l,r Indicates the error e l In the rth row of , γ is a preset threshold used to determine whether the line reprojection error is too large.

[0072] The derivative of the kernel function is taken as ρ'(el,r ), let matrix ψ=diag[ρ'(e l,r ) / e l,r ], at this time, the observation noise covariance Q can be adaptively adjusted to obtain Q adj =Q 1 / 2 Ψ -1 Q T / 2 , where Q adj is the adjusted observation noise covariance, and the superscript -1 indicates the inverse matrix. adj It is used to adaptively adjust the Kalman filter gain K, so that it can more effectively track the camera posture and camera internal and external parameters, improve the accuracy of camera state estimation, and improve positioning accuracy.

[0073] In summary, the present invention obtains Then, through the transformation matrix of the two frames i and k, we get C i The offline characteristics are in C k The estimated value under the system is finally obtained by using the projection transformation to obtain the homogeneous coordinate representation of the line feature in the pixel plane of the current frame, and the reprojection error of the line feature is obtained by observing the distance between the endpoint of the line feature and the reprojection line.

[0074] Step 2: The error update module calculates the pose error state Jacobian matrix of the line feature and corrects the state variables of the camera.

[0075] In order to achieve accurate estimation of the pose, the state needs to be updated according to the error, so the corresponding Jacobian matrix needs to be solved. This part includes the Jacobian matrix of the error to the camera extrinsic parameters, intrinsic parameters and state variables. There is no need to calculate the Jacobian for the parameter representation of the line feature, which reduces the state variables compared to the classic triangulation method. The present invention obtains the error state Jacobian matrix for the camera state variables through the chain derivation method based on the residual obtained according to formula (1.13).

[0076] Step 2.1: Get the error e l The error state Jacobian matrix for the camera pose.

[0077] Step 2.1.1: Error e l Coordinate error of the pixel plane of the line feature Find the Jacobian matrix as follows:

[0078]

[0079] In the formula, the superscript ~ indicates the error state.

[0080] Step 2.1.2: Calculate the pixel plane coordinates of line features right The error state Jacobian matrix is ​​as follows:

[0081]

[0082] The projection matrix K of the line feature can be updated by the above formula l .

[0083] Step 2.1.3: Calculation right The error state Jacobian matrix of .

[0084] The Jacobian derivation is performed through formula (1.10). In addition to the position and posture, the formula also includes the intermediate variables Therefore, we need to find the Jacobian for both.

[0085]

[0086] Step 2.1.4: Calculate the distance between the line feature and the coordinate origin For the intermediate variable A l and B l The error state Jacobian matrix of ;

[0087]

[0088] Step 2.1.5: Calculate the intermediate variable A l right The error state Jacobian matrix of ;

[0089]

[0090] Step 2.1.6: Calculate the intermediate variable B l ,Right now right The error state Jacobian matrix of ;

[0091]

[0092] Step 2.1.7: Chain derivation to get e l right The Jacobian matrix of .

[0093] According to formula (1.13), e can be determined l With each intermediate variable and The functional relationship of e l right The Jacobian matrix of is:

[0094]

[0095]

[0096]

[0097] Step 2.1.8: Calculate the Jacobian matrix of the camera pose error state to the IMU (inertial sensor) pose error state. The pose in the IMU system is usually maintained in the back-end sliding window, and there is the following conversion relationship with the pose in the camera system:

[0098]

[0099]

[0100] Let I i ,I j ,I k The corresponding IMU coordinate system is the i, j, and k frames. It is the world system to C k The rotation matrix of the camera system represents the posture of the camera in the world system at the kth frame. are the IMU postures at the i, j, and k frames in the world system, Indicates the position of the camera in the world system at the kth frame, Indicates the position of IMU in the world system at the kth frame, is the rotation matrix of the camera extrinsic parameters, C P I is the translation vector of the camera extrinsic parameter. The IMU poses at the i, j, and k frames in the world system are respectively expressed as

[0101] From this, the Jacobian matrix of the camera pose error state to the IMU pose error state can be derived:

[0102]

[0103] Where I represents the unit matrix.

[0104] Step 2.1.9: Finally get the error e l About IMU-based posture The error state Jacobian matrix of .

[0105]

[0106] The error state Jacobian matrix calculated using formula (2.10) is used to correct the position and posture of the IMU.

[0107] Step 2.2: Calculate the error e l Jacobian matrix for the camera intrinsic parameters.

[0108] Taking into account the inaccuracy of the camera's intrinsic parameters, the error e is calculated l The error state Jacobian matrix of the camera intrinsic parameters. iThe projection transformation from the system to the pixel plane, that is, equation (1.11), can be obtained For the internal parameter K'=[f x f y c x c y ]’s error state Jacobian:

[0109]

[0110] Where n(w) represents the normal vector The w+1th element in, w=0,1,2.

[0111] Combined with formula (2.1), the error e l Line feature pixel plane coordinates Jacobian, using chain derivation, we can get the error e l The Jacobian of the camera's intrinsic parameters is used to correct the camera's intrinsic parameters.

[0112] Step 2.3: Calculate the error e l Error state Jacobian matrix for camera extrinsics.

[0113] At the same time, considering the inaccuracy of the camera extrinsic parameters, the Jacobian of the error state to the camera extrinsic parameters is calculated. First, the Jacobian of the camera pose error state to the camera extrinsic parameters is derived as:

[0114]

[0115] in, Represents the camera external parameters, that is, the transformation matrix from the IMU system to the camera system C T I of error.

[0116] And through formula (2.7) we can get the error e l Camera error status The Jacobian of the error state with respect to the camera extrinsic parameters can be obtained through chain derivation as follows, thereby realizing the correction of the camera extrinsic parameters.

[0117]

[0118] Step 3: The line feature and common view frame screening module performs line feature screening and only the pose base frame (jth frame) is selected.

[0119] The pose of the line does not need to construct the Plücker coordinates of the line feature in the world system, but two frames of images need to be selected as base frames, namely the i-th and j-th frames mentioned above, to construct the reprojection error of the current frame. Previously, the i-th frame image has been selected as the image frame where the line feature is first observed. In order to select a suitable j-th frame, make the reprojection error calculation more practical, ensure the accuracy of the calculation of the intermediate variables involved, define the parallax angle of the line, and use the parallax of the line as the basis for line feature screening and base frame screening. The detailed steps are as follows.

[0120] Step 3.1: Assume that for each line feature, there are n frames of common-view observation frames between the i-th frame and the k-th frame. Traverse the intermediate frames 1 to n and use formula (1.9) to calculate respectively with the i-th frame Since the distance from the point to the straight line is greater than 0, it is eliminated unreasonable frames, so that the remaining frames Collection

[0121] Step 3.2: Calculation The mean σ and variance μ of , calculate the coefficient of variation If the coefficient of variation c is greater than or equal to the threshold c th =0.3, the line feature is regarded as an unreasonable line feature and is removed, and the next line feature is calculated in step 3.1.

[0122] Step 3.3: For line features whose coefficient of variation is less than the threshold, select the base frame to obtain the largest possible disparity so that the direction vector The solution is more accurate. To this end, the parallax angle between the two frames is defined as ψ ij :

[0123]

[0124] The absolute value L1 norm of the parallax angle of the three frames for:

[0125]

[0126] Traverse 1 to n frames of co-viewing frames and calculate their correlation with the i-th and k-th frames respectively. If ψ appears during the calculation ij If the parallax angle is less than 0.1°, it is considered that the line feature is too small, and the line feature is deleted, and the next line feature is processed in step 3.1; otherwise, select The largest observation frame is taken as the jth frame of the current line feature.

[0127] To test the method of the present invention, the reference framework is taken as the MSCKF framework to which the point feature only pose theory is applied, and the algorithm is marked as POP-MSCKF. The line feature only pose method of the present invention is applied to the reference framework to obtain a fusion algorithm, which is marked as POPL-MSCKF. The line feature measurement model is replaced with a traditional measurement model, that is, the line features are obtained through a line triangulation strategy, and the line features in the measurement equation are represented by an orthogonal representation, and the algorithm variant is named POPTL-MSCKF.

[0128] The test results of the above three methods on the EuRoc dataset are shown in Table 1. Taking the MH-05 sequence as an example, the experimental trajectory diagram and APE error are shown in Table 1. Figure 4 , Figure 5 shown.

[0129] Table 1 RMSE of positioning error of POP-MSCKF, POPL-MSCKF and POPTL-MSCKF (unit: m)

[0130] algorithm V1_01 V1_02 V1_03 V2_01 V2_02 V2_03 POP-MSCKF 0.063 0.101 0.076 0.069 0.083 0.173 POPL-MSCKF 0.078 0.094 0.072 0.068 0.083 0.144 POPTL-MSCKF 0.069 0.104 0.071 0.070 0.081 0.174 algorithm MH_01 MH_02 MH_03 MH_04 MH_05 Mean POP-MSCKF 0.116 0.156 0.251 0.166 0.300 0.141 POPL-MSCKF 0.138 0.158 0.206 0.155 0.216 0.128 POPTL-MSCKF 0.120 0.157 0.240 0.1442 0.306 0.140

[0131] From the experimental results in Table 1, it can be found that compared with the traditional triangulation method, the positioning performance of the method of the present invention is better in the five sequences, especially in complex environments such as the V2_03 and MH_05 sequences. The results in the remaining sequences are relatively close, and the overall average accuracy is significantly improved, which confirms the effectiveness of the present invention.

[0132] from Figure 6 From the roll estimation results of the V2_03 sequence, it can be found that compared with MSCKF, POPL-MSCKF can correct the state in real time, making its state update delay less, making it closer to the true value, thereby improving positioning accuracy.

[0133] Except for the technical features described in the specification, all of them are known technologies to those skilled in the art. The present invention omits the description of known components and known technologies to avoid redundancy and unnecessary limitation of the present invention. The implementation methods described in the above embodiments do not represent all implementation methods consistent with the present application. On the basis of the technical solution of the present invention, various modifications or variations that can be made by those skilled in the art without creative labor are still within the protection scope of the present invention.

Claims

1. A method for constructing a line feature pose measurement model suitable for visual SLAM, characterized in that: The steps include: Step 1: The visual front end extracts line features and uses three common view frames to construct the position and posture reprojection error of the line feature l in the current frame; Among them, the three common-view frames include frame C where the line feature l is first observed i , current frame C k And in C i and C k A common view frame C between j ; The construction method of the reprojection error of the line feature l in the current frame includes: first, through frame C i and C j Get line feature l in C i The distance parameter from the camera frame to the camera optical center as follows: Intermediate variables Among them, the line feature l is obtained in frame C i , C j The unit normal vector of the back-projection plane formed by the camera optical center and the two endpoints of the normalized plane of the line feature l under the camera C j Camera to C i The rotation matrix of the camera system, Indicates frame C j To frame C i The translation vector of ; sgn is the sign function; variable and For frame C i The coordinates of the two endpoints of the line feature l on the dedistorted pixel plane. The superscript T indicates transposition. Reuse frame C i and C k The pose relationship between them obtains the line feature l in the current frame C k The prediction result of the reprojected straight line under , let the homogeneous coordinates of the straight line be obtained Further calculate the reprojection error e of the line feature l in the current frame l ; Step 2, using the calculated reprojection error to update the current camera's pose, including: using a chain derivation method to calculate the Jacobian matrix of the reprojection error with respect to the camera pose, and the Jacobian matrix of the reprojection error with respect to the camera's extrinsic parameters and intrinsic parameters, which are used to correct the pose of the inertial sensor IMU, the camera's extrinsic parameters, and the camera's intrinsic parameters; Step 3: Set the line features extracted by the visual front end and the common viewing frame C of the line features j The screening method is to use the screened line features and the common view frame to perform the above steps 1 and 2; The common view frame is selected according to the parallax angle. For the intermediate common view frame C of the line feature j , to obtain the target with the largest parallax to select; for line features, (1) if there is an intermediate common view frame and the first observation frame C i If the parallax angle between the two frames is less than the threshold, the line feature is removed. (2) Calculate the line feature of each intermediate common view frame in C i The distance parameter from the camera system to the camera optical center is removed. The unreasonable intermediate frames whose distance parameter is less than 0 are removed. The coefficient of variation is calculated according to the mean and variance of the distance parameter of the reasonable intermediate frames. The line features whose coefficient of variation is not less than the set threshold are removed.

2. The method according to claim 1, characterized in that In step 1, constructing only the pose reprojection error through three co-viewing frames includes: Step 1.1: Get the line feature l in the image frame C i The direction vector in the camera frame; First, obtain the line feature l in frame C i , C j The unit normal vector of the back-projection plane formed by the camera optical center and the two endpoints of the normalized plane of the line feature l under the camera and will Convert to C i Camera system obtained Then according to the normal vector With line feature l in frame C i Direction vector in the camera frame Vertical relationship calculation as follows: Step 1.2: Calculate the line feature l in C i The distance from the camera frame to the camera optical center; Line feature l in C i The distance from the camera to the camera optical center There is the following relationship between the two image frames: Defining intermediate variables Further we get: in Taking the two norms on both sides of the equation, we get Specific value of Step 1.3: From frame C i and C k The pose relationship of the predicted line feature l in C k Normal vector in camera system as follows: in, C i Camera to C k The rotation matrix of the camera system, For frame C i To frame C k The translation vector of Step 1.4: The predicted line feature normal vector Project to the pixel plane to obtain the homogeneous coordinates of the line as follows: Among them, K l is the projection matrix of the line feature, f x 、f y are the focal lengths of the camera on the x-axis and y-axis, respectively, x 、c y are the horizontal and vertical positions of the camera optical center in the pixel coordinate system; set up Expressed as [I k1 I k2 I k3 ]; Step 1.5: Calculate the reprojection error e of the line feature l in the current frame l ,as follows: in, and are the homogeneous coordinates of the two endpoints of the observation line feature in the pixel plane.

3. The method according to claim 1 or 2, characterized in that: In step 1, when applied to the filtering method, according to the reprojection error e l Adaptive adjustment of the observation noise covariance includes: Calculate the Huber kernel function ρ(e l,r )as follows: Among them, e l,r Indicates the error e l The rth row of ; γ is a preset threshold used to determine whether the reprojection error is too large; Then calculate the derivative of the kernel function ρ'(e l,r ), let matrix ψ=diag[ρ'(e l,r ) / e l,r ]; Adaptively adjust the observation noise covariance Q to obtain the adjusted value Q adj =Q 1 / 2 Ψ -1 Q T / 2 , where the superscript -1 indicates the inverse matrix; the Kalman filter gain is adaptively adjusted using the adjusted observation noise covariance to update the camera state.

4. The method according to claim 1 or 2, characterized in that: In step 2, the method of obtaining the error state Jacobian matrix of the reprojection error to the camera pose includes: Step 2.1.1: Assume that the homogeneous coordinates of the line feature l corresponding to the line Homogeneous coordinates of the two endpoints of the observation line feature in the pixel plane Error e l Align minor coordinates Find the error state Jacobian matrix as follows: The superscript ~ indicates the error state; Step 2.1.2: Calculate the homogeneous coordinates of the line right The error state Jacobian matrix is ​​as follows: Where K l is the projection matrix of the line feature; Step 2.1.3: Calculation In the world system C k Camera pose The error state Jacobian matrix is: calculate In the world system C i Camera pose The error state Jacobian matrix is: calculate For intermediate variables The error state Jacobian matrix is: Step 2.1.4: Calculate the distance between the line feature and the camera optical center For the intermediate variable A l and B l The error state Jacobian matrix is: Step 2.1.5: Calculate the intermediate variable A l right The error state Jacobian matrix is: Calculate the intermediate variable A l In the world system C j Camera pose The error state Jacobian matrix is: Calculate the intermediate variable A l right The error state Jacobian matrix is: Step 2.1.6: Calculate the intermediate variable B l ,Right now To the world system to C i The rotation matrix of the camera system The error state Jacobian matrix is: Calculate intermediate variables To the world system to C j Camera pose The error state Jacobian matrix is: Step 2.1.7: Chain derivation to get e l right The error state Jacobian matrix is: Step 2.1.8: Calculate the Jacobian matrix of the camera pose error state to the IMU pose error state: Among them, let I i ,I j ,I k Corresponding frame C i , C j , C k The IMU coordinate system at time Frame C i , C j , C k IMU posture at the time; The world system to frame C i , C j , C k The rotation matrix of the IMU system; It is the rotation matrix from the IMU system to the camera system; C P I is the translation vector of the camera extrinsic parameter; I represents the unit matrix; Step 2.1.9: Calculate the error e l About IMU-based posture The error state Jacobian matrix is:

5. The method according to claim 1 or 2, characterized in that: In step 2, the error e is calculated l The Jacobian matrix of the camera's internal parameters includes: First calculate For the internal parameter K'=[f x f y c x c y ]’s error state Jacobian matrix: Where n(w) represents the normal vector The element labeled w in the Combined error e l Align minor coordinates The error state Jacobian matrix is ​​used to obtain the error e by chain derivation. l Jacobian matrix for the camera intrinsic parameters.

6. The method according to claim 1 or 2, characterized in that: In step 2, the error e is calculated l The Jacobian matrix of the camera extrinsic parameters includes: First calculate C k Camera pose Camera extrinsics C T I The error state Jacobian matrix is: Recombination error e l right The error state Jacobian matrix is ​​obtained by chain derivation. l Error state Jacobian matrix for camera extrinsics.

7. The method according to claim 1 or 2, characterized in that: The method further comprises step 3, wherein the line features extracted by the visual front end and the common viewing frame C of the line features are j To filter, the filtering method is as follows: Step 3.1: Let the first observed i-th frame C be the line feature l i With the current k-th frame C k There are n frames of common-view observation frames with line features l between them. We traverse the middle 1~n common-view frames and use C i Calculate the line feature l of each frame in C i The intermediate parameter of the distance from the camera frame to the camera optical center like The corresponding intermediate frame is unreasonable, remove the unreasonable frame, and make the remaining frames Collection Step 3.2: Calculation The mean σ and variance μ of , calculate the coefficient of variation If the coefficient of variation c is greater than or equal to the threshold c th =0.3, the line feature is regarded as an unreasonable line feature and is removed, and the next line feature is calculated in step 3.1; Step 3.3: For the coefficient of variation c less than the threshold c th Line features, using the parallax angle for base frame C j Selection of Define two frames C i and C j The parallax angle between ij , calculated as follows: in, They are the line features in frame C i , C j The unit normal vector of the back-projection plane in the camera system; It is C j Camera to C i The rotation matrix of the camera system; Calculate three frames C i , C j , C k The absolute value L1 norm of the parallax angle for: where ψ jk It is two frames C j and C k The parallax angle between ki It is two frames C k and C i The parallax angle between Traverse 1 to n frames of common view frames, and calculate the absolute value L1 norm of the disparity angle between each frame and the i-th frame and the k-th frame respectively If ψ appears ij If the parallax angle is less than 0.1°, it is considered that the parallax angle is too small, and the line feature is deleted, and the next line feature is calculated again in step 3.1; Otherwise select The largest common view frame is used as the base frame C of the current line feature j .

Images