A method for constructing a line feature-only pose measurement model suitable for visual SLAM

Abstract

The present invention discloses a method for constructing a line feature pose-only measurement model suitable for visual SLAM, which belongs to the field of SLAM. Traditional SLAM line features need to be triangulated when constructing reprojection errors. The introduced triangulation error reduces the positioning accuracy, and as the line features increase, the state variables increase accordingly. In response to these problems, the method of the present invention uses the line features extracted by the visual front end to construct only the pose reprojection error, and uses three-frame constraints to construct the reprojection error of the line features in the current frame. A corresponding error state Jacobian solution method for the line feature reprojection error with respect to the state variable is given, and a line feature screening and pose-only base frame selection method is provided to improve positioning accuracy. Integrating the method of the present invention into a point-only pose system for use can effectively improve positioning accuracy and reduce the number of state variables. When applied under the MSCKF framework, real-time correction of the current frame state of the system can be achieved.

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 also become increasingly mature. However, based solely on point features, the problem of reduced positioning accuracy caused by the sparse feature points in weak texture scenes has been exposed. To solve this problem, scholars have 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, triangulates the line features, and reprojects them, and uses the reprojection error to construct constraints. However, this inevitably introduces triangulation errors of 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, as the number of line features increases, the number of state variables also increases. To address these problems, the present invention proposes a line feature-only pose measurement model construction method suitable for visual SLAM based on a point feature-only pose method. The method is integrated into the point feature-only pose VINS (Visual-Inertial Navigation System), and line reprojection is directly constructed through three-frame observations, so that the reprojection error variables only contain the system state variables. 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 positioning accuracy.

[0004] The present invention provides a method for constructing a line feature-only pose measurement model suitable for visual SLAM, comprising 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 An 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 is used to obtain 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 line feature l under the camera C j Camera connected to C i The rotation matrix of the camera system, Indicates frame C j To frame C i The translation vector; sgn is the sign function; variable and 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 base frame selection. j Screening; Among them, the common view frame is selected according to the parallax angle, and the intermediate common view frame C of the line feature is selected. j , to obtain the target with the largest parallax to select; for line features, (1) if the 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 to the camera optical center is removed. The unreasonable intermediate frames with distance parameters less than 0 are eliminated. The coefficient of variation is calculated based on the mean and variance of the distance parameters of the reasonable intermediate frames. The line features with the coefficient of variation not less than the set threshold are eliminated.

[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 of 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 experiments 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 under 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 1 is a schematic diagram of the overall process of the method for constructing a line feature pose 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 diagram used in the method of the present invention to construct the reprojection error of the line feature;

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

[0019] Figure 5 1 is a trajectory APE error diagram of an embodiment of the present invention applying the method of the present invention and the comparative method to 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 in an 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 pose-only measurement model of line features applicable to 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-only 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 uses the line features extracted by the vision front end to construct the pose-only reprojection error.

[0024] In visual odometry, it is necessary to construct feature constraints to solve the camera pose. The same is true for the introduction of line features. Before this, it is clear that the straight line description method used in this 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 line in the current frame. The subscript e indicates that the vector is a unit vector. l Indicates the distance from the spatial 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 known, the Plücker coordinate representation of the line can be obtained:

[0026]

[0027] Where, [] × Indicates the antisymmetric matrix, n l 、v l They are the normal vector and direction vector of the back-projection plane of 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 common view 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 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 k-th 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 view 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 composed of the camera optical center of the line feature l in the i-th, j-th, and k-th frame camera systems and the two endpoints of the line feature normalization plane is obtained respectively. and Will and All converted to C i The camera is mounted. 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 this as follows:

[0034]

[0035] Where 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 constraint of the starting point and end point of the line segment, that is, to have a unified direction, the variable ζ is defined as follows:

[0037]

[0038] Where, and 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's optical center

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

[0041]

[0042] Where, 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's optical center.

[0043] because The denominator term is included in the equation, which makes the Jacobian solution more complicated. In order to reduce the state variables in the denominator, we define the intermediate variables

[0044]

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

[0046]

[0047] Where, is defined as:

[0048]

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

[0050]

[0051] Where, 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 the camera system

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

[0054]

[0055] Where, 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, we have calculated the line features in C. k Normal vector in the 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] Where 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, c x 、c y are the horizontal and vertical positions of the camera's optical center in the pixel coordinate system, respectively.

[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, so the reprojection error e of the current line feature in the kth frame is l It can be expressed as:

[0062]

[0063] Where, 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 obtained line feature reprojection error, e l It does not include parameters related to line feature calculation, and 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 using the method of the present invention is and It's 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 are system state transfer equations and state observation equations. The state observation equation is: Z k =Hx k +v k Among them, Z k is the observation 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: v 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] Where, e l,r Represents 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 to make it more effective in tracking the camera pose and camera internal and external parameters, thereby improving the accuracy of camera state estimation and positioning accuracy.

[0073] In summary, the present invention obtains Then, we get C through the transformation matrix of the two frames i and k. i The line characteristics are in C k The estimated value under the system is obtained, and finally the homogeneous coordinate representation of the line feature in the current frame pixel plane is obtained by using the projection transformation. The reprojection error of the line feature is obtained by observing the distance between the end point 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] To achieve accurate pose estimation, the state needs to be updated based on 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 features, 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 Error state Jacobian matrix for the camera pose.

[0077] Step 2.1.1: Error e l Coordinate error of the line feature pixel plane 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), which includes intermediate variables in addition to the posture 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 derivative 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 Functional relationship, we get 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 the pose in the camera system has the following conversion relationship:

[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 expressed as

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

[0102]

[0103] Where I represents the identity matrix.

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

[0105]

[0106] Use the error state Jacobian matrix calculated by formula (2.10) 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 internal parameters, the error e is calculated l The error state Jacobian matrix of the camera internal parameters. iThe projection transformation from the system to the pixel plane, that is, formula (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 is w=0,1,2.

[0111] Combined with formula (2.1), the error e l Line feature pixel plane coordinates Jacobian, the error e can be obtained by chain derivation 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 The error state Jacobian matrix for the camera extrinsics.

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

[0114]

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

[0116] And through formula (2.7) we can get the error e l Camera error status The Jacobian matrix of the error state with respect to the camera extrinsic parameters can be obtained by 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 a line does not require constructing the Plücker coordinates of the line feature in the world system. However, two image frames must be selected as base frames, namely the i-th and j-th frames mentioned above, to construct the reprojection error of the current frame. The i-th frame has been selected as the image frame where the line feature was first observed. To select the appropriate j-th frame, ensure a more realistic reprojection error calculation and accurate calculation of the intermediate variables involved, define the line's parallax angle, and use the line's parallax as the basis for line feature selection and base frame selection. 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 middle 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 The collection is

[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 with a coefficient of variation less than the threshold, select the base frame to obtain the largest possible parallax, 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 L1 norm of the absolute value of the parallax angle of the three frames for:

[0125]

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

[0127] To test the proposed method, the baseline framework was the MSCKF framework, which applies the point feature pose-only theory. This algorithm was labeled POP-MSCKF. The proposed line feature pose-only method was applied to this baseline framework, resulting in a fusion algorithm labeled POPL-MSCKF. The line feature measurement model was replaced with a traditional measurement model, where line features were obtained through a line triangulation strategy and the line features in the measurement equations were represented using an orthogonal representation. This algorithm variant was named POPTL-MSCKF.

[0128] The test results of the above three methods on the EuRoc dataset are shown in Table 1. Taking the dataset 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 errors 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, we can see that compared with the traditional triangulation method, the proposed method has better positioning performance in the five sequences, especially in complex environments such as the V2_03 and MH_05 sequences. The results are relatively close in the remaining sequences, and the overall average accuracy is significantly improved, confirming the effectiveness of the proposed method.

[0132] from Figure 6 From the roll angle 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 and closer to the true value, thereby improving positioning accuracy.

[0133] Except for the technical features described in the specification, all other technical features are known to those skilled in the art. The present invention omits descriptions of well-known components and well-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. Based on the technical solution of the present invention, various modifications or variations that can be made by those skilled in the art without creative effort are still within the scope of protection of the present invention.

Claims

1. A method for constructing a line feature-only pose measurement model suitable for visual SLAM, characterized in that: The steps include: Step 1: The visual front-end extracts line features and constructs the pose reprojection error of the line feature l in the current frame using three common view frames. 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 line feature l under the camera C j Camera connected 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; [] × Indicates the antisymmetric matrix; Reuse frame C i and C k The pose relationship between them is used to obtain 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 ; The calculated line feature l in the current frame reprojection error e l include: 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 line feature l under the camera and will Convert to C i Get the camera 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's 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 gives 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 the camera system as follows: in, C i Camera connected 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, c x 、c y are the horizontal and vertical positions of the camera's 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; Step 2: Use the calculated reprojection error to update the current camera pose, including: using the chain derivation method to calculate the error state 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 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; Step 3: Set the line features extracted by the visual front end and the common viewing frame C of the line features j The filtering method is to use the filtered line features and the common view frame to perform steps 1 and 2 above; 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 the 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 to the camera optical center is removed. The unreasonable intermediate frames with distance parameters less than 0 are eliminated. The coefficient of variation is calculated based on the mean and variance of the distance parameters of the reasonable intermediate frames. The line features with the coefficient of variation not less than the set threshold are eliminated.

2. The method according to claim 1, characterized in that In step 1, when applied to the filtering method, according to the reprojection error e l Adaptive adjustment of observation noise covariance, including: Calculate the Huber kernel function ρ(e l,r )as follows: Among them, e l,r Represents 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.

3. The method according to claim 1, characterized in that In step 2, the method for 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 secondary 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 of : calculate In the world system C i Camera pose The error state Jacobian matrix of : calculate For intermediate variables The error state Jacobian matrix of : 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 of : Step 2.1.5: Calculate the intermediate variable A l right The error state Jacobian matrix of : Calculate the intermediate variable A l In the world system C j Camera pose The error state Jacobian matrix of : Calculate the intermediate variable A l right The error state Jacobian matrix of : Step 2.1.6: Calculate the intermediate variable B l ,Right now World System to C i Camera rotation matrix The error state Jacobian matrix of : Calculate intermediate variables World System to C j Camera pose The error state Jacobian matrix of : Step 2.1.7: Chain derivative to get e l right The error state Jacobian matrix of : 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 pose at time ; World system to frame C i 、C j 、C k The rotation matrix of the IMU system; 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 of :

4. The method according to claim 1, wherein In step 2, the error e is calculated l The Jacobian matrix of the camera's intrinsic 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 with label w; Combined error e l Align secondary coordinates The error state Jacobian matrix is ​​obtained by chain derivation. l Jacobian matrix for the camera intrinsic parameters.

5. The method according to claim 1, wherein 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 of : Recombination error e l right The error state Jacobian matrix is ​​obtained by chain derivation. l The error state Jacobian matrix for the camera extrinsics.

6. The method according to claim 1, wherein 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 line feature l, the first observation i-th frame C i With the current k-th frame C k There are n frames of common viewing observation frames with line features l between them. Traverse the middle 1~n common viewing 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 The collection is Step 3.2: Calculate C i The mean σ and variance μ of D, 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 considered 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 parallax angle for base frame C j Selection; 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 under the camera frame; It is C j Camera connected 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~n frames of common viewing frames and calculate the L1 norm of the absolute value 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 too small, the line feature is deleted, and step 3.1 is repeated to calculate the next line feature. Otherwise select The largest common view frame is used as the base frame C of the current line feature j .

Images