Visual inertia fusion positioning method and device
By fully stacking and matrix merging, building and eliminating the models of multiple feature points in multiple camera states, the problem of large amount of computation is solved, and the efficiency and accuracy of visual inertial fusion positioning is improved.
Patent Information
- Application Number
- CN202310517600.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2023-05-09
- Publication Date
- 2024-11-12
AI Technical Summary
The traditional Kalman Filter (MSCKF) algorithm under multi-state constraints has a large amount of calculation during the state update process, which affects the positioning efficiency.
By fully stacking and matrix merging a single model of multiple feature points in multiple camera states, the product terms of the first-order partial derivative matrix and the system state matrix are constructed, projected and eliminated, and the second-order partial derivative matrix is obtained. The residual model then performs state updates, avoiding left zero space calculation and QR decomposition dimensionality reduction calculation.
Saves calculation amount, improve positioning efficiency, reduce CPU overhead, and improve computing efficiency and positioning accuracy.
Smart Images

Figure CN118936459A_ABST
Abstract
Description
Technical Field
[0001] The embodiments of the present disclosure relate to the field of navigation and positioning technology, and in particular to a visual-inertial fusion positioning method and device. Background Art
[0002] Visual Inertial Odometry (VIO) is an algorithm that integrates the data of camera and inertial measurement unit (IMU) to achieve simultaneous localization and mapping (SLAM). It can be divided into two categories: filtering-based and optimization-based. Multi-State Constraint Kalman Filter (MSCKF) is a filtering-based VIO algorithm.
[0003] However, the inventors have found that the traditional MSCKF algorithm has at least the following technical problems: the amount of calculation during the state update process is large, which affects the positioning efficiency. Summary of the invention
[0004] The embodiments of the present disclosure provide a visual-inertial fusion positioning method and device to overcome the problem that the MSCKF algorithm has a large amount of calculation during the state update process, which affects the positioning efficiency.
[0005] In a first aspect, an embodiment of the present disclosure provides a visual-inertial fusion positioning method, including:
[0006] Obtain target observation data corresponding to the target state vector; the target observation data includes observation data of at least one feature point; the observation data of each feature point includes observation data of the feature point under at least one camera state;
[0007] A first residual model is constructed according to the target observation data; the first residual model includes a product term of a first-order partial derivative matrix and a system state matrix, and a first observation noise term; the first-order partial derivative matrix includes a first first-order partial derivative matrix obtained by differentiating the target state vector and a second first-order partial derivative matrix obtained by differentiating the three-dimensional coordinate vector of the at least one feature point; the system state matrix includes a target state error vector and a three-dimensional coordinate error vector of the feature point; the target state vector includes an inertial measurement unit IMU state vector and a camera state vector;
[0008] According to the first-order partial derivative matrix, the residual model is projected to obtain a second residual model; the second residual model includes a product term of the second-order partial derivative matrix and the system state matrix, and a covariance term corresponding to the first observation noise term;
[0009] Eliminate the three-dimensional coordinate error vector of the second residual model to obtain a target residual model, and perform EKF update on the target state vector according to the target residual model.
[0010] In a second aspect, an embodiment of the present disclosure provides a visual inertial fusion positioning device, including:
[0011] An acquisition module, used to acquire target observation data corresponding to a target state vector; the target observation data includes observation data of at least one feature point; the observation data of each feature point includes observation data of the feature point under at least one camera state; the target state vector includes multiple camera states;
[0012] A construction module, used to construct a first residual model according to the target observation data; the first residual model includes a product term of a first-order partial derivative matrix and a system state matrix, and a first observation noise term; the first-order partial derivative matrix includes a first first-order partial derivative matrix obtained by differentiating the target state vector and a second first-order partial derivative matrix obtained by differentiating the three-dimensional coordinate vector of the at least one feature point; the system state matrix includes a target state error vector and a three-dimensional coordinate error vector of the feature point; the target state vector includes an inertial measurement unit IMU state vector and a camera state vector;
[0013] A projection module, configured to project the residual model according to the first-order partial derivative matrix to obtain a second residual model; the second residual model includes a product term of a second-order partial derivative matrix and a system state matrix, and a covariance term corresponding to the first observation noise term;
[0014] An elimination module is used to eliminate the three-dimensional coordinate error vector of the second residual model to obtain a target residual model, and perform EKF update on the target state vector according to the target residual model.
[0015] In a third aspect, an embodiment of the present disclosure provides an electronic device, including: a processor and a memory;
[0016] The memory stores computer-executable instructions;
[0017] The processor executes the computer-executable instructions stored in the memory, so that the at least one processor executes the visual-inertial fusion positioning method as described in the first aspect and various possible designs of the first aspect.
[0018] In a fourth aspect, an embodiment of the present disclosure provides a computer-readable storage medium, in which computer execution instructions are stored. When a processor executes the computer execution instructions, the visual-inertial fusion positioning method described in the first aspect and various possible designs of the first aspect is implemented.
[0019] In a fifth aspect, an embodiment of the present disclosure provides a computer program product, including a computer program, which, when executed by a processor, implements the visual-inertial fusion positioning method described in the first aspect and various possible designs of the first aspect.
[0020] The visual-inertial fusion positioning method and device provided in this embodiment first obtain target observation data corresponding to a target state vector, the target observation data including observation data of at least one feature point, the observation data of each feature point including observation data of the feature point under at least one camera state, and construct a first residual model according to the target observation data, the first residual model including a product term of a first-order partial derivative matrix and a system state matrix, and a first observation noise term, the first-order partial derivative matrix including a first first-order partial derivative matrix obtained by differentiating a target state vector and a second first-order partial derivative matrix obtained by differentiating a three-dimensional coordinate vector of at least one feature point, the system state matrix including a target state error vector and a three-dimensional coordinate error vector of the feature point, the target state vector including an inertial measurement unit IMU state vector and a camera state vector, projecting the residual model according to the first-order partial derivative matrix to obtain a second residual model, the second residual model including a product term of a second-order partial derivative matrix and a system state matrix, and a covariance term corresponding to the first observation noise term, eliminating the three-dimensional coordinate error vector of the second residual model to obtain a target residual model, and updating the target state vector according to the target residual model. In this embodiment, a first residual model is obtained by fully stacking and matrix merging a single model of multiple feature points in multiple camera states, and then projecting it to obtain a second residual model including a second-order partial derivative matrix, and then eliminating the second residual model to obtain a target residual model for state update. This calculation process does not involve left null space calculation and projection, and QR decomposition dimensionality reduction calculation, which saves calculation amount and improves positioning efficiency. BRIEF DESCRIPTION OF THE DRAWINGS
[0021] In order to more clearly illustrate the embodiments of the present disclosure or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present disclosure. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.
[0022] Figure 1A schematic diagram of the principle of a visual-inertial fusion positioning method provided in an embodiment of the present disclosure;
[0023] Figure 2 Schematic diagram of the visual inertial fusion positioning method provided in the embodiment of the present disclosure Figure 1 ;
[0024] Figure 3 A schematic diagram of the observation relationship between multiple feature points and multiple camera states in the current sliding window corresponding to the target state vector provided in an embodiment of the present disclosure;
[0025] Figure 4 Schematic diagram of the visual inertial fusion positioning method provided in the embodiment of the present disclosure Figure 2 ;
[0026] Figure 5 A schematic diagram of the observation relationship between a plurality of feature points corresponding to a target state vector provided in an embodiment of the present disclosure and a plurality of camera states in a current sliding window and a plurality of historical camera states in a common viewing window;
[0027] Figure 6 A structural block diagram of a visual-inertial fusion positioning device provided in an embodiment of the present disclosure;
[0028] Figure 7 A schematic diagram of the hardware structure of the visual-inertial fusion positioning device provided in an embodiment of the present disclosure. DETAILED DESCRIPTION
[0029] In order to make the purpose, technical solution and advantages of the embodiments of the present disclosure clearer, the technical solution in the embodiments of the present disclosure will be clearly and completely described below in conjunction with the drawings in the embodiments of the present disclosure. Obviously, the described embodiments are part of the embodiments of the present disclosure, not all of the embodiments. Based on the embodiments in the present disclosure, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present disclosure.
[0030] Visual Inertial Odometry (VIO) is an algorithm that integrates the data of camera and inertial measurement unit (IMU) to achieve simultaneous localization and mapping (SLAM). It can be divided into two categories: filtering-based and optimization-based. Multi-State Constraint Kalman Filter (MSCKF) is a filtering-based VIO algorithm. However, the amount of calculation of MSCKF in the state update process is large, which affects the positioning efficiency. Specifically, in the state update, it is necessary to perform left null space calculation and left null space projection on each feature point observation to be updated, and because the number of rows in the matrix in the model obtained after the left null space projection far exceeds the number of columns, it is also necessary to use the QR decomposition method for dimensionality reduction. This series of complex calculations will cause a lot of CPU overhead in the link again, affecting the calculation efficiency and positioning efficiency.
[0031] In order to solve the above technical problems, the inventors of the present application have found that the first residual model obtained by fully stacking and matrix merging a single model of multiple feature points in multiple camera states can be projected, including a second residual model of a second-order partial derivative matrix, and then the second residual model is eliminated to obtain a target residual model for state update. The calculation process does not involve left null space calculation and projection, as well as QR decomposition dimensionality reduction calculation, which saves calculation amount and improves positioning efficiency. Based on this, the embodiment of the present disclosure provides a visual inertial fusion positioning method.
[0032] refer to Figure 1 , Figure 1 The following is a schematic diagram of the principle of a visual inertial fusion positioning method provided by an embodiment of the present disclosure. Figure 1As shown, the visual-inertial fusion system includes a positioning device 101, an IMU 102, and a camera 103. Among them, the positioning device 101 adopts an extended Kalman filter (Extended Kalman Filter, EKF) framework, performs EKF prediction based on the IMU data (IMU readings) sensed by the IMU 102, and updates the EKF based on the image frames taken by the camera 103, that is, the observation data of the feature points. Specifically, based on the camera 103, the camera state at different times (for example, the camera 103 position P and the attitude quaternion Q) are added to the state vector, and the feature points can be seen by multiple cameras 103, thereby forming a geometric constraint (Constraint) between multiple camera states (Multi-State), and then the geometric constraint can be used to construct a residual model, so as to update the EKF. The positioning device 101 can be applied to robots, drones, augmented reality technology (Augmented Reality, AR) / virtual reality technology (Virtual Reality, VR) and other fields.
[0033] In the specific implementation process, the state vector and covariance are first initialized. Then, in the prediction phase, based on each IMU reading, the system basic state and system covariance are propagated from image k to image k+1, usually processing multiple IMU readings. In the update phase, after the camera 103 acquires the image frame at time k+1, it calculates the current camera state based on the image frame, and adds the current camera state to the state vector to obtain the target state vector, while expanding the state covariance. The target residual model is constructed based on the observation data corresponding to the target state vector, and then the EKF is updated based on the target residual model.
[0034] Specifically, in constructing a target residual model, target observation data corresponding to a target state vector is obtained; the target observation data includes observation data of at least one feature point; the observation data of each feature point includes observation data of the feature point under at least one camera state; a first residual model is constructed according to the target observation data; the first residual model includes a product term of a first-order partial derivative matrix and a system state matrix, and a first observation noise term; the first-order partial derivative matrix includes a first first-order partial derivative matrix obtained by differentiating the target state vector and a second first-order partial derivative matrix obtained by differentiating the three-dimensional coordinate vector of the at least one feature point. derivative matrix; the system state matrix includes a target state error vector and a three-dimensional coordinate error vector of a feature point; the target state vector includes an inertial measurement unit IMU state vector and a camera state vector; according to the first-order partial derivative matrix, the residual model is projected to obtain a second residual model; the second residual model includes a product term of a second-order partial derivative matrix and a system state matrix, and a covariance term corresponding to the first observation noise term; the three-dimensional coordinate error vector of the second residual model is eliminated to obtain a target residual model, and the target state vector is updated by EKF according to the target residual model.
[0035] The visual-inertial fusion positioning method provided by the disclosed embodiment projects a first residual model obtained by fully stacking and matrix merging a single model of multiple feature points in multiple camera states to obtain a second residual model including a second-order partial derivative matrix, and then eliminates the second residual model to obtain a target residual model for state update. The calculation process does not involve left null space calculation and projection, and QR decomposition dimensionality reduction calculation, which saves calculation amount and improves positioning efficiency.
[0036] refer to Figure 2 , Figure 2 Schematic diagram of the visual inertial fusion positioning method provided in the embodiment of the present disclosure Figure 1 The method of this embodiment can be applied in Figure 1 In the positioning device shown, the visual inertial fusion positioning method includes:
[0037] 201. Obtain target observation data corresponding to a target state vector; the target observation data includes observation data of at least one feature point; the observation data of each feature point includes observation data of the feature point under at least one camera state.
[0038] In the embodiment of the present disclosure, the target state vector is the system state vector when the update timing determined by the update strategy is reached. I Including the base state X A (IMU status) and augmented status X B (Camera status), basic status XA It can include the attitude, displacement, velocity, accelerometer bias, gyroscope bias of the IMU, and can also include the rotational and / or translational external parameters from the camera to the IMU. B It can include the rotation, displacement, and speed of the camera.
[0039] In one embodiment of the present disclosure, in order to ensure that the system state vector does not increase infinitely and affect the computational efficiency, an upper limit value of the number of expansions of the sliding window (i.e., sliding window) of the system state vector can be set. For example, the upper limit value is set to n, then X I =[X A , X B0 , X B1 , …, X Bs ], when s=n, before continuing to increase or expand, it is necessary to delete the existing increase or expansion states in the sliding window to ensure that the number of increase or expansion states in the sliding window is less than or equal to the upper limit value n. The specific deletion strategy (the number of deletions, the deletion position, etc.) can be determined according to the actual situation, and the embodiments of the present disclosure do not limit this.
[0040] In the embodiment of the present disclosure, the observation data of the feature point in the camera state is the relevant data of the image frame containing the feature point captured by the camera in the camera state. The content of the target observation data is limited according to the adopted update strategy.
[0041] Exemplarily, in one implementable method, all observation data in the sliding window corresponding to the feature points that cannot be observed in the current sliding window can be determined as target observation data, and then a residual model is constructed based on the target observation data and an extended Kalman filter (EKF) is used for state update.
[0042] In another possible implementation, the upper limit of the number of augmented states in the sliding window can be set to m. When s==m, all observations of the first two or more camera states in the sliding window in chronological order, or all observations of the last two or more camera states, or all observations of one or more camera states before and after, are extracted as needed, and the extracted observation data are determined as target observation data, and then a residual model is constructed based on the target observation data to use EKF for state update. As for which windows are extracted for state update, a comprehensive decision can be made based on whether the current system is moving or whether the amount of movement (such as movement distance) reaches a specific limit).
[0043] In another possible implementation, in order to improve the calculation accuracy, based on any of the above methods, in addition to the target observation data determined by the above methods, the observation data of the historical camera state outside the current sliding window, that is, earlier than the earliest time in the sliding window and associated with at least one feature point in the sliding window, can also be determined as the target observation data. For details, please refer to the introduction of the subsequent embodiments, which will not be repeated here.
[0044] refer to Figure 3 , Figure 3 The following is a schematic diagram of the observation relationship between multiple feature points and multiple camera states in the current sliding window corresponding to the target state vector provided in the embodiment of the present disclosure. Figure 3 As shown in the figure, the five-pointed star represents the feature point, the box represents the augmented camera state sequence, and the dotted line represents the observation relationship between the feature point and the camera state. The current sliding window corresponding to the target state vector includes the feature point f 0 to f 4 , the camera status includes Ca 0 To Ca 4 . Feature point f 0 Being in five camera states (Ca 0 To Ca 4 ) is observed by the camera under the feature point f 1 Being in three camera states (Ca 2 To Ca 4 ) is observed by the camera under the feature point f 2 Being in four camera states (Ca 1 To Ca 4 ) is observed by the camera under the feature point f 3 Being in three camera states (Ca 2 To Ca 4 ) is observed by the camera under the feature point f 4 Being in three camera states (Ca 2 To Ca 4 ) is observed by the camera under.
[0045] In some public embodiments, before constructing the first residual model according to the target observation data, it may also include: obtaining multiple IMU data after the previous moment and before the current moment; predicting the system state vector at the previous moment according to the multiple IMU data to obtain the system state vector to be augmented; obtaining the image frame at the current moment, calculating the camera state vector corresponding to the image frame according to the image frame, and augmenting the system state vector to be augmented based on the camera state vector to obtain the target state vector.
[0046] Specifically, in the prediction stage, multiple IMU data between image time k and time k+1 can be integrated to propagate the system basic state and system covariance from image time k to image time k+1. After obtaining the image frame at time k+1, the camera state is calculated and augmented to obtain the target state vector.
[0047] 202. Construct a first residual model according to the target observation data; the first residual model includes a product term of a first-order partial derivative matrix and a system state matrix, and a first observation noise term; the first-order partial derivative matrix includes a first first-order partial derivative matrix obtained by differentiating the target state vector and a second first-order partial derivative matrix obtained by differentiating the three-dimensional coordinate vector of at least one feature point; the system state matrix includes a target state error vector and a three-dimensional coordinate error vector of the feature point; the target state vector includes an inertial measurement unit IMU state vector and a camera state vector.
[0048] Specifically, the feature points can be triangulated based on the camera state in the current sliding window (i.e., the three-dimensional coordinates of the feature points can be fitted), and then the observation data of multiple feature points under multiple camera states can be determined based on a preset state update strategy (refer to the example description of step 201 above), so as to construct a first residual model based on the three-dimensional coordinates of the multiple feature points and the corresponding observation data of the multiple camera states.
[0049] In the process of triangulation estimation, the camera states of multiple different perspectives (different positions) and the observation data of the feature points corresponding to the camera states can be used to solve the three-dimensional coordinates of the feature points based on the camera state based on the nonlinear optimization algorithm. After the three-dimensional coordinates of the feature points in the camera state are obtained by triangulation estimation, the three-dimensional coordinates of the feature points in the world coordinate system can be calculated based on the three-dimensional coordinates and the position relationship of the camera in the world coordinate system. Where W represents the world coordinate system, P represents the displacement, and i represents the i-th feature point f.
[0050] In some disclosed embodiments, constructing a first residual model based on the target observation data may include: for each feature point among the at least one feature point and each camera state among the at least one camera state corresponding to the feature point, constructing a third residual model according to the observation data of the feature point under the camera state in the target observation data; the third residual model includes a product term of a first partial derivative matrix and a target state error vector, a product term of a second partial derivative matrix and a three-dimensional coordinate error vector of the feature point, and a second observation noise term; the first partial derivative matrix is a first-order partial derivative of the residual function with respect to the target state vector, and the second partial derivative matrix is a first-order partial derivative of the residual function with respect to the three-dimensional coordinate vector of the feature point; stacking the third residual models corresponding to multiple feature points to obtain a fourth residual model; and performing matrix merging on the product terms in the fourth residual model to obtain the first residual model.
[0051] In some disclosed embodiments, constructing a third residual model for each feature point of the at least one feature point and each camera state of the at least one camera state corresponding to the feature point according to the observation data of the feature point in the target observation data under the camera state may include: constructing a residual function for each feature point of the at least one feature point and each camera state of the at least one camera state corresponding to the feature point according to the observation data of the feature point in the target observation data under the camera state; the residual function is related to the target state vector and the three-dimensional coordinate vector of the feature point; and linearizing the residual function to obtain a third residual model.
[0052] In some disclosed embodiments, stacking the third residual models corresponding to the plurality of feature points to obtain the fourth residual model may include: stacking the third residual models corresponding to the feature points of the at least one camera state to obtain the fifth residual model; and stacking the fourth residual models corresponding to the at least one feature point to obtain the fourth residual model.
[0053] In the disclosed embodiment, the first-order partial derivative matrix may be a Jacobian matrix.
[0054] Exemplarily, the constraint in vision refers to the reprojection error of the feature point to the camera, that is, the error between the two-dimensional coordinates of the feature point actually observed by the camera and the two-dimensional coordinates of the estimated three-dimensional coordinates of the feature point projected onto the image.
[0055] First, the residual function r = f(X I , P f ), where r is the residual, X I is the target state vector, Pf is the three-dimensional coordinate of the feature point.
[0056] Secondly, based on the observation of the i-th feature point under the j-th camera state, the third residual model is constructed:
[0057]
[0058] Among them, r i,j represents the observation residual of the i-th feature point under the j-th camera state; J x(i,j) Represents the Jacobian matrix of the residual for the target state vector, that is, the first partial derivative matrix, represents the system state error, that is, the target state error vector, J f(i,j) It represents the Jacobian matrix of the residual corresponding to the three-dimensional coordinates of the i-th feature point, that is, the second partial derivative matrix, It represents the small amount of three-dimensional coordinate error of the i-th feature point in the world coordinate system, that is, the three-dimensional coordinate error vector, n i,j It represents the observation noise of the system on the i-th feature point in the j-th camera state, that is, the second observation noise.
[0059] Again, based on all the observation data in the camera state sequence specified by the i-th feature point in the sliding window, a residual model is constructed, that is, Formula 1 is stacked to form a fourth residual model of the following matrix mode:
[0060]
[0061] Among them, r i represents the arrangement or stacking of the observation residuals of the i-th feature point in all specified camera states; J x(i) It means that the residual is the arrangement or stacking of the Jacobian matrix of the system state, indicating that the system state error is small. It means that the residual pair corresponds to the arrangement or stacking of the Jacobian matrix of the three-dimensional coordinates of the i-th feature point. Indicates that the three-dimensional coordinate error of the i-th feature point in the world coordinate system is small, n i represents the stacked combination of the observation noise of the system for the i-th feature point under all specified camera states.
[0062] Again, based on all observations of the feature points that can be used for state update under the relevant camera states within the sliding window, a fifth residual model is constructed:
[0063]
[0064] Among them, J x Indicates that the observation residuals of all the specified feature points in all sliding windows are used to calculate the Jacobian matrix of the system state variables, J fRepresents the observation residuals of all the specified feature points in all sliding windows to find the Jacobian matrix of the three-dimensional coordinates of all the feature points used to construct the residuals, and n represents the first observation noise:
[0065] Again, the fifth residual model can be matrix merged to obtain the first residual model:
[0066]
[0067] It can be seen that the system Jacobian matrix in the above formula (4) is J = [J x J f ], the system status is Represents the error between the basic state and the augmented state of the system, Represents the error of the three-dimensional coordinates of all the specified feature points; however, the EKF system state variable itself does not contain the three-dimensional coordinates of any feature point. Therefore, it needs to be eliminated later.
[0068] To better understand the meaning of formula (4), the following is combined with Figure 3 Formula (4) is explained with an example. Assume s = 4, the camera pose sequence in the current sliding window can observe feature points 0, 1, 2, 3, and 4. There are 5 augmented camera states in the current sliding window. The observation relationship between feature points and camera states is as follows: Figure 3 Therefore, formula (4) can be expressed in the following form.
[0069]
[0070] The corresponding covariance R is:
[0071]
[0072] 203. Project the residual model according to the first-order partial derivative matrix to obtain a second residual model; the second residual model includes a product term of the second-order partial derivative matrix and the system state matrix, and a covariance term corresponding to the first observation noise term.
[0073] Specifically, in order to obtain the second-order partial derivative matrix, the matrix to be projected can be determined based on the dimension of the first-order partial derivative matrix. For example, the transposed matrix of the first-order partial derivative matrix can be determined as the matrix to be projected.
[0074] For example, in order to perform fast elimination, the first residual model shown in formula (4) can be first projected onto [J x J f ] T , and the following second residual model is obtained.
[0075]
[0076] 204. Eliminate the three-dimensional coordinate error vector of the second residual model to obtain a target residual model, and perform EKF update on the target state vector according to the target residual model.
[0077] Specifically, after obtaining a second residual model including a square matrix by projecting the first residual model, Gaussian elimination method (specifically, for example, Schur elimination method) can be used to eliminate the three-dimensional coordinate error vector in the system state matrix in the second residual model.
[0078] For example, in formula (6), n is a vector, and the values of each member are usually set to be the same. Each element of n is denoted as u. Formula (6) can be simplified as follows:
[0079]
[0080] Right now:
[0081]
[0082] In formula (8) is the variable that needs to be eliminated. Formula (8) can be projected into L space The following residual model is obtained:
[0083]
[0084] After arranging and simplifying, we can get:
[0085]
[0086] Since the EKF system state only contains the camera pose but not the three-dimensional coordinates of the feature points, we can get:
[0087]
[0088] From formula (11), we can see that the target residual model after elimination can be organized into the following standard form.
[0089]
[0090] Therefore, the EKF can be updated based on formula (12) and combined with the following formula:
[0091] The gain calculation formula is:
[0092] K=PJ T (JPJ T +R) -1 (13)
[0093] Where K represents the Kalman gain, P is the system covariance, and J is the Jacobian matrix.
[0094] The state correction calculation formula is:
[0095] ΔX=Kr (14)
[0096] The state update formula is:
[0097] X=X+ΔX (15)
[0098] The covariance update formula is:
[0099] P=(I-KJ)P(I-KJ) T +KRK T (16)
[0100] In some disclosed embodiments, after the EKF is updated based on the above formula, the observation data corresponding to the feature points used for updating will be set so that the observation data is no longer used for subsequent EKF updates. After setting, the update result, such as the update result of the target state vector, can be output.
[0101] The disclosed embodiments of the above formulas (1) to (16) project the residual model (including the noise model) with the non-existent state variables to the Jacobi transposed space, and after being projected to the Jacobi transposed space, project it to a specific space (such as the space related to the Schur elimination method), and obtain a system state that can directly eliminate the unnecessary parts of the system state mathematically (that is, the derivation result contains a 0 matrix multiplied by the unnecessary parts of the system state), so that the projected residual model does not contain the unnecessary parts of the system state. The noise in the residual model is projected by the above-mentioned Schur elimination matrix to the observation noise model n of the target state variable, and then the EKF is updated. It is achieved that the positioning efficiency is improved while ensuring the positioning accuracy.
[0102] From the above description, it can be seen that the first residual model obtained by fully stacking and matrix merging a single model of multiple feature points in multiple camera states is projected to obtain a second residual model including a second-order partial derivative matrix, and then the second residual model is eliminated to obtain a target residual model for state updating. The calculation process does not involve left null space calculation and projection, as well as QR decomposition dimensionality reduction calculation, which saves calculation amount and improves positioning efficiency.
[0103] refer to Figure 4 , Figure 4 Schematic diagram of the visual inertial fusion positioning method provided in the embodiment of the present disclosure Figure 2 In order to improve the calculation accuracy, the present embodiment adds the application of observation data in the common viewing window. The visual inertial fusion positioning method includes:
[0104] 401. Obtain first observation data of at least one feature point in a current sliding window corresponding to a target state vector, obtain second observation data of at least one feature point in at least one historical camera state outside the current sliding window, and determine the first observation data and the second observation data as target observation data.
[0105] 402. For each feature point of the at least one feature point and each historical camera state of the at least one historical camera state corresponding to the feature point, a sixth residual model is constructed according to the observation data of the feature point under the historical camera state in the target observation data; the sixth residual model includes a product term of a third partial derivative matrix and a three-dimensional coordinate error vector of the feature point, and a third observation noise term; the third partial derivative matrix is a first-order partial derivative of a residual function constructed with the historical camera state as a constant with respect to the three-dimensional coordinate vector of the feature point.
[0106] 403. Stack the third residual model and the sixth residual model corresponding to the plurality of feature points to obtain a fourth residual model.
[0107] 404. Perform matrix merging on the product terms in the fourth residual model to obtain a first residual model.
[0108] Specifically, the feature points in the current sliding window may have historical camera states that have an observation relationship with them outside the current sliding window. If these historical camera states are added to the constraints on the three-dimensional coordinates of the feature points, the amount of observation data can be increased, which helps to improve the accuracy of the first-order partial derivative of the three-dimensional coordinates, thereby improving the accuracy of the residual model and the accuracy of the EKF update.
[0109] Exemplary, reference Figure 5 , Figure 5 A schematic diagram of the observation relationship between multiple feature points corresponding to the target state vector provided in the embodiment of the present disclosure and multiple camera states in the current sliding window and multiple historical camera states in the common viewing window. Figure 5 As shown, in Figure 3 Based on the observation relationship shown in the figure, the feature point f in the common window is added 0 The historical camera state Ca- 2 To Ca -1 The common view window refers to a window of historical camera states outside the current sliding window that has an observation relationship with at least one feature point in the sliding window. Then, the feature point f can be added to the residual model 0 The historical camera state Ca- 2 To Ca -1 The following residual model is obtained:
[0110]
[0111] By comparing formula (17) with formula (5), it can be found that the observation data based on the common view window adds constraints on the three-dimensional coordinate error vector of the feature point.
[0112] The disclosed embodiment adds the residual formed by the landmark points observed within the current sliding window and the observations of the old window outside the sliding window to the current residual construction, and sets the posture (position and attitude) of the old window to a constant, so that the residual constrains the current EKF solution, making the EKF solution more accurate and the positioning accuracy higher.
[0113] 405. Project the residual model according to the first-order partial derivative matrix to obtain a second residual model; the second residual model includes a product term of a second-order partial derivative matrix and the system state matrix, and a covariance term corresponding to the first observation noise term;
[0114] 406. Eliminate the three-dimensional coordinate error vector of the second residual model to obtain a target residual model, and perform EKF update on the target state vector according to the target residual model.
[0115] Steps 405 to 406 in the disclosed embodiment are similar to steps 203 to 204 in the disclosed embodiment described above, and are not described again here.
[0116] From the above description, it can be seen that by adding the observation data of the historical camera state outside the current sliding window, that is, in the common view window, which has an observation relationship with at least one feature point in the current sliding window, into the construction of the residual model, the amount of observation data can be increased, which helps to improve the accuracy of the first-order partial derivative of the three-dimensional coordinates, thereby improving the accuracy of the residual model and the accuracy of the EKF update.
[0117] Corresponding to the visual inertial fusion positioning method in the above embodiment, Figure 6 This is a structural block diagram of a visual inertial fusion positioning device provided in an embodiment of the present disclosure. For ease of explanation, only the parts related to the embodiment of the present disclosure are shown. Figure 6 The device 60 includes: an acquisition module 601, a construction module 602, a projection module 603 and an elimination module 604.
[0118] Wherein, the acquisition module 601 is used to acquire target observation data corresponding to the target state vector; the target observation data includes observation data of at least one feature point; the observation data of each feature point includes observation data of the feature point under at least one camera state; the target state vector includes multiple camera states;
[0119] A construction module 602 is used to construct a first residual model according to the target observation data; the first residual model includes a product term of a first-order partial derivative matrix and a system state matrix, and a first observation noise term; the first-order partial derivative matrix includes a first first-order partial derivative matrix obtained by differentiating the target state vector and a second first-order partial derivative matrix obtained by differentiating the three-dimensional coordinate vector of the at least one feature point; the system state matrix includes a target state error vector and a three-dimensional coordinate error vector of the feature point; the target state vector includes an inertial measurement unit IMU state vector and a camera state vector;
[0120] A projection module 603 is used to project the residual model according to the first-order partial derivative matrix to obtain a second residual model; the second residual model includes a product term of the second-order partial derivative matrix and the system state matrix, and a covariance term corresponding to the first observation noise term;
[0121] The elimination module 604 is used to eliminate the three-dimensional coordinate error vector of the second residual model to obtain a target residual model, and perform EKF update on the target state vector according to the target residual model.
[0122] In one embodiment of the present disclosure, the construction module 602 is specifically used to: for each feature point in the at least one feature point and each camera state in the at least one camera state corresponding to the feature point, construct a third residual model according to the observation data of the feature point in the camera state in the target observation data; the third residual model includes a product term of a first partial derivative matrix and a target state error vector, a product term of a second partial derivative matrix and a three-dimensional coordinate error vector of the feature point, and a second observation noise term; the first partial derivative matrix is a first-order partial derivative of the residual function with respect to the target state vector, and the second partial derivative matrix is a first-order partial derivative of the residual function with respect to the three-dimensional coordinate vector of the feature point; stack the third residual models corresponding to multiple feature points to obtain a fourth residual model; and perform matrix merging on the product terms in the fourth residual model to obtain the first residual model.
[0123] The construction module 602 is specifically used to: for each feature point in the at least one feature point and each camera state in the at least one camera state corresponding to the feature point, construct a residual function according to the observation data of the feature point in the camera state in the target observation data; the residual function is related to the target state vector and the three-dimensional coordinate vector of the feature point; and linearize the residual function to obtain a third residual model.
[0124] The construction module 602 is specifically used to: stack the third residual models corresponding to the feature points of the at least one camera state to obtain a fifth residual model; and stack the fourth residual models corresponding to the at least one feature point to obtain a fourth residual model.
[0125] The acquisition module 601 is specifically used to: acquire first observation data of at least one feature point in the current sliding window corresponding to the target state vector; acquire second observation data of at least one of the feature points under at least one historical camera state outside the current sliding window; and determine the first observation data and the second observation data as target observation data.
[0126] The construction module 602 is also specifically used to: for each feature point in the at least one feature point and each historical camera state in the at least one historical camera state corresponding to the feature point, construct a sixth residual model according to the observation data of the feature point in the historical camera state in the target observation data; the sixth residual model includes the product term of the third partial derivative matrix and the three-dimensional coordinate error vector of the feature point, and the third observation noise term; the third partial derivative matrix is the first-order partial derivative of the residual function constructed with the historical camera state as a constant with respect to the three-dimensional coordinate vector of the feature point; the third residual models and the sixth residual models corresponding to multiple feature points are stacked to obtain a fourth residual model.
[0127] The device 60 also includes a prediction and augmentation module for: obtaining multiple IMU data after the previous moment and before the current moment; predicting the system state vector at the previous moment based on the multiple IMU data to obtain the system state vector to be augmented; obtaining the image frame at the current moment, calculating the camera state vector corresponding to the image frame based on the image frame, and augmenting the system state vector to be augmented based on the camera state vector to obtain the target state vector.
[0128] The elimination module 604 is specifically used to: based on the Schur elimination method, eliminate the three-dimensional coordinate error vector of the second residual model to obtain the target residual model.
[0129] The device provided in this embodiment can be used to execute the technical solution of the above method embodiment. Its implementation principle and technical effect are similar, and this embodiment will not be repeated here.
[0130] In order to implement the above embodiment, the embodiment of the present disclosure also provides an electronic device.
[0131] refer to Figure 7, which shows a schematic diagram of the structure of an electronic device 700 suitable for implementing the embodiment of the present disclosure, and the electronic device 700 may be a terminal device or a server. The terminal device may include but is not limited to mobile terminals such as mobile phones, notebook computers, digital broadcast receivers, personal digital assistants (PDAs), tablet computers (Portable Android Devices, PADs), portable multimedia players (PMPs), vehicle terminals (such as vehicle navigation terminals), etc., and fixed terminals such as digital TVs, desktop computers, etc. Figure 7 The electronic device shown is only an example and should not bring any limitation to the functions and scope of use of the embodiments of the present disclosure.
[0132] like Figure 7 As shown, the electronic device 700 may include a processing device (e.g., a central processing unit, a graphics processing unit, etc.) 701, which can perform various appropriate actions and processes according to a program stored in a read-only memory (ROM) 702 or a program loaded from a storage device 708 to a random access memory (RAM) 703. Various programs and data required for the operation of the electronic device 700 are also stored in the RAM 703. The processing device 701, the ROM 702, and the RAM 703 are connected to each other via a bus 704. An input / output (I / O) interface 705 is also connected to the bus 704.
[0133] Typically, the following devices may be connected to the I / O interface 705: input devices 706 including, for example, a touch screen, a touch pad, a keyboard, a mouse, a camera, a microphone, an accelerometer, a gyroscope, etc.; output devices 707 including, for example, a liquid crystal display (LCD), a speaker, a vibrator, etc.; storage devices 708 including, for example, a magnetic tape, a hard disk, etc.; and communication devices 709. The communication devices 709 may allow the electronic device 700 to communicate with other devices wirelessly or by wire to exchange data. Although Figure 7 The electronic device 700 is shown with various devices, but it should be understood that it is not required to implement or possess all the devices shown. More or fewer devices may be implemented or possessed instead.
[0134] In particular, according to an embodiment of the present disclosure, the process described above with reference to the flowchart can be implemented as a computer software program. For example, an embodiment of the present disclosure includes a computer program product, which includes a computer program carried on a computer-readable medium, and the computer program includes a program code for executing the method shown in the flowchart. In such an embodiment, the computer program can be downloaded and installed from a network through a communication device 709, or installed from a storage device 708, or installed from a ROM 702. When the computer program is executed by the processing device 701, the above-mentioned functions defined in the method of the embodiment of the present disclosure are executed.
[0135] It should be noted that the computer-readable medium disclosed above may be a computer-readable signal medium or a computer-readable storage medium or any combination of the above two. The computer-readable storage medium may be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device or device, or any combination of the above. More specific examples of computer-readable storage media may include, but are not limited to: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In the present disclosure, a computer-readable storage medium may be any tangible medium containing or storing a program that may be used by or in combination with an instruction execution system, device or device. In the present disclosure, a computer-readable signal medium may include a data signal propagated in a baseband or as part of a carrier wave, in which a computer-readable program code is carried. This propagated data signal may take a variety of forms, including but not limited to an electromagnetic signal, an optical signal, or any suitable combination of the above. The computer readable signal medium may also be any computer readable medium other than a computer readable storage medium, which may send, propagate or transmit a program for use by or in conjunction with an instruction execution system, apparatus or device. The program code contained on the computer readable medium may be transmitted using any suitable medium, including but not limited to: wires, optical cables, RF (radio frequency), etc., or any suitable combination of the above.
[0136] The computer-readable medium may be included in the electronic device, or may exist independently without being installed in the electronic device.
[0137] The computer-readable medium carries one or more programs. When the one or more programs are executed by the electronic device, the electronic device executes the method shown in the above embodiment.
[0138] Computer program code for performing the operations of the present disclosure may be written in one or more programming languages or a combination thereof, including object-oriented programming languages, such as Java, Smalltalk, C++, and conventional procedural programming languages, such as "C" or similar programming languages. The program code may be executed entirely on the user's computer, partially on the user's computer, as an independent software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In the case of a remote computer, the remote computer may be connected to the user's computer via any type of network, including a local area network (LAN) or a wide area network (WAN), or may be connected to an external computer (e.g., via the Internet using an Internet service provider).
[0139] The flow chart and block diagram in the accompanying drawings illustrate the possible architecture, function and operation of the system, method and computer program product according to various embodiments of the present disclosure. In this regard, each square box in the flow chart or block diagram can represent a module, a program segment or a part of a code, and the module, the program segment or a part of the code contains one or more executable instructions for realizing the specified logical function. It should also be noted that in some implementations as replacements, the functions marked in the square box can also occur in a sequence different from that marked in the accompanying drawings. For example, two square boxes represented in succession can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each square box in the block diagram and / or flow chart, and the combination of the square boxes in the block diagram and / or flow chart can be implemented with a dedicated hardware-based system that performs a specified function or operation, or can be implemented with a combination of dedicated hardware and computer instructions.
[0140] The units involved in the embodiments described in the present disclosure may be implemented by software or hardware. The name of a unit does not limit the unit itself in some cases. For example, the first acquisition unit may also be described as a "unit for acquiring at least two Internet Protocol addresses".
[0141] The functions described above herein may be performed at least in part by one or more hardware logic components. For example, without limitation, exemplary types of hardware logic components that may be used include: field programmable gate arrays (FPGAs), application specific integrated circuits (ASICs), application specific standard products (ASSPs), systems on chips (SOCs), complex programmable logic devices (CPLDs), and the like.
[0142] In the context of the present disclosure, a machine-readable medium may be a tangible medium that may contain or store a program for use by or in conjunction with an instruction execution system, device, or equipment. A machine-readable medium may be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium may include, but is not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, device, or equipment, or any suitable combination of the foregoing. A more specific example of a machine-readable storage medium may include an electrical connection based on one or more lines, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.
Claims
1. A visual inertial fusion positioning method, characterized in that: include: Obtain target observation data corresponding to the target state vector; The target observation data includes observation data of at least one feature point; the observation data of each feature point includes observation data of the feature point under at least one camera state; A first residual model is constructed according to the target observation data; the first residual model includes a product term of a first-order partial derivative matrix and a system state matrix, and a first observation noise term; the first-order partial derivative matrix includes a first first-order partial derivative matrix obtained by differentiating the target state vector and a second first-order partial derivative matrix obtained by differentiating the three-dimensional coordinate vector of the at least one feature point; the system state matrix includes a target state error vector and a three-dimensional coordinate error vector of the feature point; the target state vector includes an inertial measurement unit IMU state vector and a camera state vector; According to the first-order partial derivative matrix, the residual model is projected to obtain a second residual model; the second residual model includes a product term of the second-order partial derivative matrix and the system state matrix, and a covariance term corresponding to the first observation noise term; Eliminate the three-dimensional coordinate error vector of the second residual model to obtain a target residual model, and perform EKF update on the target state vector according to the target residual model.
2. The method according to claim 1, characterized in that The step of constructing a first residual model according to the target observation data comprises: For each feature point in the at least one feature point and each camera state in the at least one camera state corresponding to the feature point, a third residual model is constructed according to the observation data of the feature point in the camera state in the target observation data; the third residual model includes a product term of a first partial derivative matrix and a target state error vector, a product term of a second partial derivative matrix and a three-dimensional coordinate error vector of the feature point, and a second observation noise term; the first partial derivative matrix is a first-order partial derivative of the residual function with respect to the target state vector, and the second partial derivative matrix is a first-order partial derivative of the residual function with respect to the three-dimensional coordinate vector of the feature point; Stacking the third residual models corresponding to the plurality of feature points to obtain a fourth residual model; The product terms in the fourth residual model are matrix-merged to obtain a first residual model.
3. The method according to claim 2, characterized in that The constructing, for each feature point of the at least one feature point and each camera state of the at least one camera state corresponding to the feature point, a third residual model according to observation data of the feature point in the camera state in the target observation data, comprises: For each feature point of the at least one feature point and each camera state of the at least one camera state corresponding to the feature point, construct a residual function according to the observation data of the feature point in the camera state in the target observation data; the residual function is related to the target state vector and the three-dimensional coordinate vector of the feature point; The residual function is linearized to obtain a third residual model.
4. The method according to claim 2, characterized in that: The step of stacking the third residual models respectively corresponding to the plurality of feature points to obtain a fourth residual model comprises: stacking the third residual models of the feature points corresponding to the at least one camera state to obtain a fifth residual model; The fourth residual models corresponding to the at least one feature point are stacked to obtain a fourth residual model.
5. The method according to claim 2, characterized in that: The obtaining of target observation data corresponding to the target state vector includes: Obtaining first observation data of at least one feature point in the current sliding window corresponding to the target state vector; Acquire second observation data of at least one of the feature points under at least one historical camera state outside the current sliding window; The first observation data and the second observation data are determined as target observation data.
6. The method according to claim 5, characterized in that The constructing of the first residual model according to the target observation data further includes: For each feature point of the at least one feature point and each historical camera state of the at least one historical camera state corresponding to the feature point, a sixth residual model is constructed according to the observation data of the feature point in the historical camera state in the target observation data; the sixth residual model includes a product term of a third partial derivative matrix and a three-dimensional coordinate error vector of the feature point, and a third observation noise term; the third partial derivative matrix is a first-order partial derivative of a residual function constructed with the historical camera state as a constant with respect to the three-dimensional coordinate vector of the feature point; The step of stacking the third residual models respectively corresponding to the plurality of feature points to obtain a fourth residual model comprises: The third residual model and the sixth residual model corresponding to the plurality of feature points are stacked to obtain a fourth residual model.
7. The method according to any one of claims 1 to 6, characterized in that: Before constructing the first residual model according to the target observation data, the method further includes: Get multiple IMU data after the previous moment and before the current moment; Predicting the system state vector at the previous moment according to the plurality of IMU data to obtain the system state vector to be augmented; An image frame at a current moment is acquired, a camera state vector corresponding to the image frame is calculated according to the image frame, and the system state vector to be augmented is augmented based on the camera state vector to obtain a target state vector.
8. The method according to any one of claims 1 to 6, characterized in that: Eliminating the three-dimensional coordinate error vector of the second residual model to obtain a target residual model includes: Based on the Schur elimination method, the three-dimensional coordinate error vector of the second residual model is eliminated to obtain a target residual model.
9. A visual inertial fusion positioning device, characterized in that: include: An acquisition module is used to acquire target observation data corresponding to a target state vector; The target observation data includes observation data of at least one feature point; the observation data of each feature point includes observation data of the feature point under at least one camera state; the target state vector includes multiple camera states; A construction module, used to construct a first residual model according to the target observation data; the first residual model includes a product term of a first-order partial derivative matrix and a system state matrix, and a first observation noise term; the first-order partial derivative matrix includes a first first-order partial derivative matrix obtained by differentiating the target state vector and a second first-order partial derivative matrix obtained by differentiating the three-dimensional coordinate vector of the at least one feature point; the system state matrix includes a target state error vector and a three-dimensional coordinate error vector of the feature point; the target state vector includes an inertial measurement unit IMU state vector and a camera state vector; A projection module, configured to project the residual model according to the first-order partial derivative matrix to obtain a second residual model; the second residual model includes a product term of a second-order partial derivative matrix and a system state matrix, and a covariance term corresponding to the first observation noise term; An elimination module is used to eliminate the three-dimensional coordinate error vector of the second residual model to obtain a target residual model, and perform EKF update on the target state vector according to the target residual model.
10. An electronic device, characterized in that: include: Processor and memory; The memory stores computer-executable instructions; The processor executes the computer-executable instructions stored in the memory, so that the processor executes the visual-inertial fusion positioning method according to any one of claims 1 to 8.
11. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer-executable instructions. When the processor executes the computer-executable instructions, the visual-inertial fusion positioning method according to any one of claims 1 to 8 is implemented.
12. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the method for visual-inertial fusion positioning according to any one of claims 1 to 8 is implemented.
