Error suppression method and device for planar auxiliary vision-inertial odometer
By combining the inertial measurement unit and the RGB-D camera, using an extended Kalman filter to update the point and plane features and detect plane drift, the problem of plane feature association difficulties in visual inertial odometer systems is solved, and positioning accuracy and map consistency are improved , cope with cumulative drift.
Patent Information
- Application Number
- CN202411001022.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-24
- Publication Date
- 2024-11-08
AI Technical Summary
The existing visual inertial odometer system is difficult to effectively correlate plane features when facing drift, resulting in a decrease in positioning accuracy, especially in the case of serious cumulative drift problems in long-term navigation.
By combining the inertial measurement unit and the RGB-D camera, point and plane features are updated using an extended Kalman filter, and the similarity matrix is calculated based on the plane included angle and distance to detect drift, fixed drift-free road marking planes to suppress errors.
It effectively reduces system drift, improves positioning accuracy and map consistency, especially in long-term navigation, which can better cope with cumulative drift and provide more geometric measurement information.
Smart Images

Figure CN118913261A_ABST
Abstract
Description
Technical Field
[0001] The present disclosure relates to the field of computer vision, and more specifically, to an error suppression method and device for a planar-assisted visual-inertial odometer. Background Art
[0002] Most existing visual-inertial odometry (VIO) / simultaneous localization and mapping (SLAM) systems rely on sparse point features for efficiency and robustness considerations. RGB-D cameras simplify the task of triangulating point features and extracting high-level features. Compared with point features, planar features can provide complementary information to improve performance, especially in challenging scenarios (e.g., when point features degrade under weak texture conditions). In addition, planar features are ubiquitous in man-made environments and are more interpretable and usable in providing structural representations. Therefore, many recent studies have focused on combining point and planar features.
[0003] One of the key issues in studying planes as landmarks in RGB-DVIO / SLAM systems is data association. Unlike point features tracked on 2D images, planes are usually associated based on their spatial parameters in a unified coordinate system. Generally, when the angle and interval between two planes are within a defined threshold, they are considered a matching pair. In addition, some studies have also used plane covariance and Mahalanobis distance methods for plane association. Since these methods are highly dependent on the initial pose of the system, once drift occurs, the planes cannot be successfully associated, which weakens their value in providing long-term constraints. Summary of the invention
[0004] In order to solve the above problems, the present disclosure proposes an error suppression method and device for a planar-assisted visual-inertial odometer, a computing system, and a computer-readable storage medium.
[0005] According to one aspect of the present disclosure, there is provided an error suppression method for a plane-assisted visual-inertial odometry, the plane-assisted visual-inertial odometry comprising an inertial measurement unit and an RGB-D camera, the error suppression method comprising: determining a state vector of the inertial measurement unit based on measurement values of a local rotational velocity and a local translational acceleration obtained by the inertial measurement unit; determining point features and plane features based on a color map and a depth map obtained by the RGB-D camera; performing updates on the point features and plane features by using an extended Kalman filter determined based on the state vector; determining a similarity matrix based on an angle and a distance between two sets of planes, and determining whether an observed plane has drift based on the similarity matrix; and fixing a previously created drift-free landmark plane in response to determining that the observed plane has drift.
[0006] Optionally, the step of determining point features and plane features includes: for the color map, using the FAST corner detection algorithm to extract feature points, and using the sparse optical flow tracking method to track the feature points; for the depth map, using the aggregation hierarchical clustering algorithm to extract plane features, and associating them with the plane in the map according to the plane normal and distance factor.
[0007] Optionally, the step of performing an update on the point features includes: classifying the point features into synchronous positioning and mapping point features and polymorphic constrained Kalman function point features according to the tracked length; for the synchronous positioning and mapping point features, adding them to the state vector and updating their coordinates; for the polymorphic constrained Kalman function point features, using null space projection to eliminate the dependence on them before updating.
[0008] Optionally, the step of performing update on the plane features includes: performing update on all the plane features as synchronous positioning and mapping plane features.
[0009] Optionally, the similarity matrix includes a vertex similarity matrix and an edge similarity matrix, the vertex similarity matrix is determined based on the angle and distance between two groups of planes, and the edge similarity matrix is determined based on the difference between the angle and distance between the two groups of planes.
[0010] Optionally, the error suppression method further comprises: after fixing the previously created drift-free landmark plane, updating the landmark plane by enforcing pairwise equality constraints.
[0011] According to another aspect of the present disclosure, an error suppression device for a plane-assisted visual-inertial odometry is provided, wherein the plane-assisted visual-inertial odometry includes an inertial measurement unit and an RGB-D camera, and the error suppression device includes: an integration and propagation unit, configured to determine a state vector of the inertial measurement unit based on measurement values of a local rotational velocity and a local translational acceleration obtained by the inertial measurement unit; a feature detection and association unit, configured to determine point features and plane features based on a color map and a depth map obtained by the RGB-D camera; an extended Kalman filter update unit, configured to perform an update on the point features and plane features through an extended Kalman filter determined based on the state vector; a drift detection unit, configured to determine a similarity matrix based on an angle and a distance between two groups of planes, and determine whether an observed plane drifts based on the similarity matrix; and an error suppression unit, configured to fix a previously created drift-free landmark plane in response to determining that the observed plane drifts.
[0012] Optionally, the feature detection and association unit is configured to: for color images, extract feature points using the FAST corner detection algorithm, and track the feature points using the sparse optical flow tracking method; for depth images, extract plane features using the agglomerated hierarchical clustering algorithm, and associate them with the planes in the map based on the plane normal and distance factor.
[0013] Optionally, the extended Kalman filter update unit is configured to: classify point features into synchronous positioning and mapping point features and polymorphic constrained Kalman function point features according to the tracked length; for the synchronous positioning and mapping point features, add them to the state vector and update their coordinates; for the polymorphic constrained Kalman function point features, use null space projection to eliminate the dependence on them before updating them.
[0014] Optionally, the extended Kalman filter update unit is configured to update all plane features as synchronous positioning and mapping plane features.
[0015] Optionally, the similarity matrix includes a vertex similarity matrix and an edge similarity matrix, the vertex similarity matrix is determined based on the angle and distance between two groups of planes, and the edge similarity matrix is determined based on the difference between the angle and distance between the two groups of planes.
[0016] Optionally, the error suppression unit is further configured to: update the landmark plane by enforcing a pairwise equality constraint after fixing a previously created drift-free landmark plane.
[0017] According to another aspect of the present disclosure, a plane-assisted visual-inertial odometry system is provided, comprising: an inertial measurement unit; an RGB-D camera; and a controller configured to execute the error suppression method for plane-assisted visual-inertial odometry as described above.
[0018] According to another aspect of the present disclosure, a computing system is provided that includes at least one computing device and at least one storage device storing instructions, wherein the instructions, when executed by the at least one computing device, cause the at least one computing device to execute the error suppression method for planar-assisted visual-inertial odometry as described above.
[0019] According to yet another aspect of the present disclosure, a computer-readable storage medium storing instructions is provided, wherein when the instructions are executed by at least one computing device, the at least one computing device is prompted to perform the error suppression method for planar-assisted visual-inertial odometry as described above.
[0020] By adopting the present disclosure, it is possible to achieve: based on the original multi-state constrained Kalman function (MSCKF) framework, effectively integrate depth information and plane measurement, use the depth of the point to improve the feature triangulation result, and use the nearest point (CP) parameterized plane measurement and depth measurement are also designed and integrated into the EKF update process; make full use of the different spatial relationships of the plane patch, and apply a graph-based strategy for drift suppression. As a result, the system's performance in dealing with cumulative drift can be improved (especially in long-term navigation). For indoor structured areas, more geometric metric information can be provided, which can be used to further constrain motion. Due to its extensive spatial and temporal observability, it can effectively reduce system drift. BRIEF DESCRIPTION OF THE DRAWINGS
[0021] The above and / or other objects and advantages of the present disclosure will become more apparent through the following description of embodiments in conjunction with the accompanying drawings, in which:
[0022] Figure 1 is a flow chart illustrating an error suppression method for a plane-assisted visual-inertial odometry according to an exemplary embodiment of the present disclosure;
[0023] Figure 2 is a block diagram illustrating a workflow of a planar-assisted visual-inertial odometry according to an example of the present disclosure;
[0024] Figure 3 shows accurate trajectory and consistent floor plan results obtained using drift suppression strategies according to the present disclosure;
[0025] Figure 4 is a diagram showing different positional relationships between planar patches;
[0026] Figure 5 is a diagram illustrating an error suppression flow according to an example of the present disclosure;
[0027] Figure 6 The visualization results of the positioning trajectory on the robot sequence are shown;
[0028] Figure 7 Shown are the floor plan and trajectory visualization results constructed on a long corridor sequence;
[0029] Figure 8 is a block diagram illustrating an error suppression apparatus for a plane-assisted visual-inertial odometry according to an exemplary embodiment of the present disclosure;
[0030] Fig. 9 is a block diagram illustrating a computing system including at least one computing device and at least one storage device storing instructions according to an exemplary embodiment of the present disclosure. DETAILED DESCRIPTION
[0031] Below, in conjunction with the accompanying drawings, a description of a specific embodiment is provided to help the reader obtain a comprehensive understanding of the method, device and / or system described herein. However, after understanding the disclosure of the present application, various changes, modifications and equivalents of the method, device and / or system described herein will be clear. For example, the order of operations described herein is only an example and is not limited to those orders set forth herein, but can be changed as will be clear after understanding the disclosure of the present application, except for operations that must occur in a specific order. In addition, for greater clarity and simplicity, the description of features known in the art may be omitted.
[0032] In order to make full use of long-standing planes, the present disclosure proposes an error suppression method for plane-assisted visual-inertial odometry, the key idea of which is to understand the structural regularity of a given scene and perform drift detection by identifying repeated plane structures in the map. In other words, if one set of planes overlaps with another set of planes to some extent, and their spatial configurations are similar, the map will become inconsistent, indicating potential drift. Once drift is detected, an attempt is made to suppress it and correct the pose accordingly. The method according to the present disclosure can cope with large drifts and robustly associate repeated planes to improve positioning accuracy and ground Figure 1 Consistency.
[0033] Figure 1 is a flow chart illustrating an error suppression method for a plane-assisted visual-inertial odometry according to an exemplary embodiment of the present disclosure. The plane-assisted visual-inertial odometry may include an inertial measurement unit and an RGB-D camera.
[0034] like Figure 1 As shown, in step S101, based on the measurement values of the local rotation speed and the local translation acceleration obtained by the inertial measurement unit, the state vector of the inertial measurement unit is determined.
[0035] In the example, the system state is propagated forward to the next moment through the IMU. Using the 6-axis inertial measurement unit to provide measurements of local rotational velocity and local translational acceleration, the IMU motion model can be expressed as:
[0036] Iω m (t)=Iω(t)+b g (t)+n g (t) (1)
[0037]
[0038] Among them, b a (t) and b g (t) represent the deviations of the accelerometer and gyroscope, respectively, n a (t) and n g(t) represent the noise of accelerometer and gyroscope respectively, Represents the rotation matrix from the world coordinate system to the IMU coordinate system, G g is the gravity in the world coordinate system.
[0039] The IMU state vector is defined as:
[0040]
[0041] The five items represent: the rotation quaternion from the world coordinate system to the IMU, the position of the IMU, the speed in the IMU coordinate system, the gyroscope bias, and the accelerometer bias.
[0042] The error propagation consists of two parts: the state error is propagated to the next moment, and the measurement noise of the current moment is propagated to the next moment. Therefore, the error state propagation formula can be expressed as:
[0043]
[0044] in, represents the state error, n represents the measurement noise, Φ and G are the system Jacobian matrix and the noise Jacobian matrix respectively. Further expanding the error, it can be expressed as:
[0045]
[0046] b g,k+1 =b g,k +n wg,k (8)
[0047] b a,k+1 =b a,k +n wa,k (9)
[0048] From the above error state propagation formula, the propagation formula of the state error covariance can be obtained as follows:
[0049]
[0050] The state is augmented using the above formula, the IMU posture at the current moment is added to the state variable, and the covariance matrix is augmented. In the example, the derivation of the above formulas (1) to (10) can be achieved by existing methods.
[0051] In step S102, point features and plane features are determined based on the color image and depth image obtained by the RGB-D camera. In the example, for the color image, the FAST (Features from Accelerated Segment Test) corner detection algorithm is used to extract feature points, and the sparse optical flow tracking method is used to track the feature points; for the depth image, the agglomeration hierarchical clustering algorithm is used to extract plane features, and they are associated with the planes in the map according to the plane normal and the distance factor. Specifically, Figure 2 As shown in the figure, the input images are color images and depth images. First, the color image is histogram equalized to enhance the contrast, and the FAST corner points are used for feature extraction. In order to ensure the uniformity of distribution, the corner points are sorted according to the response value. The image is gridded during extraction, and then a uniform number of feature points are extracted in each grid, and then sub-pixel optimization is performed. The feature point tracking uses the KLT optical flow tracking method, and then the matching points are dedistorted and the RANSAC method is used to remove the features of the tracking error. Each time before tracking, the reference image needs to be re-extracted to ensure that there are a sufficient number of features. At the same time, the agglomerative hierarchical clustering (AHC) algorithm is used to extract the plane in the depth map, and it is associated with the plane in the map according to the plane normal and distance factor.
[0052] In step S103, the point features and plane features are updated by using an extended Kalman filter determined based on the state vector. For example, point features are classified into synchronous positioning and mapping point features and polymorphic constrained Kalman function point features according to the length being tracked; for synchronous positioning and mapping point features, they are added to the state vector and their coordinates are updated; for polymorphic constrained Kalman function point features, the dependency on them is eliminated by using null space projection before updating. All plane features are updated as synchronous positioning and mapping plane features.
[0053] In the example, the specific process of updating the extended Kalman filter (EKF) is as follows.
[0054] In t k At this moment, the state of the system is defined as:
[0055]
[0056] in, is the IMU state, x calib is the intrinsic and extrinsic parameters of the camera, x C is the IMU pose in the window, x P and x ΠThey are point features and surface features respectively. The filter update is divided into two parts: point update and plane update. When the depth value of the key point can be successfully observed, the environmental point will be used as a 3D observation. If there are a sufficient number of observations, the point feature update step is performed. First, the 3D position of the point needs to be obtained. To this end, a Cartesian triangulation technology that fuses depth information is proposed. Specifically, the camera pose obtained by IMU propagation is used as a known quantity. When a point feature is observed by a camera, the following equation is satisfied:
[0057]
[0058] in, Represents the 3D point coordinates in the camera coordinate system, Indicates the depth of the point. Represents the observation disturbance vector at this point.
[0059] If the depth of the point is known, it can be directly converted to world coordinates. Otherwise, the following transformation relationship is obtained with the help of the perturbation vector:
[0060]
[0061] in, Represents the rotation matrix from the world coordinate system to the camera coordinate system, Represents the position of the camera in the world coordinate system, G f i Represents the coordinates of a 3D point in the world coordinate system, G b f represents the perturbation vector of the point in the camera coordinate system, Represents the vector after rotation to the world coordinate system.
[0062] Definition and G b f Orthogonal vector N m , and substituting it into formula (13) we get the following formula.
[0063]
[0064] Stacking all the mixed features can form a linear system of equations, and the 3D position of the point feature can be solved according to the system of equations.
[0065] After obtaining the 3D coordinates, the next step is to calculate the 3D observation model of the point. The general point observation function can be expressed as:
[0066] z b =h(x k )+n b (15)
[0067] Equation (15) means that the pointG f i Project it onto the current image to get the 2D pixel coordinate z b , n b is the observation noise, h(x k ) represents the camera projection function, which is based on the state x k Project the 3D point into the pixel coordinate system to get the projection coordinate z b The observation function after introducing depth can be expressed as:
[0068]
[0069] Among them, z d is the depth observation, n d is the observation noise. Stacking the two into a 3D observation equation and linearizing the above formula can obtain the following residual equation:
[0070]
[0071] in, is the observation error between the projected point and the observed point, is the error in the state vector, is the 3D point error, and are the Jacobian matrices of observation relative to the state and three-dimensional points, n m is the observation noise. All mixed observations at each time stamp are stacked up and EKF updates are performed for all point features. Point features are classified into SLAM features and MSCKF features according to the length they are tracked. For SLAM features with longer tracks, they are added to the state vector and their coordinates are updated. For MSCKF features with shorter tracks, the null space projection is used to eliminate their dependence before performing standard EKF updates.
[0072] For plane features, since their parametric equations can be directly extracted from RGB-D sequences, they are used as direct 3D observations to update the system state. The plane adopts the nearest point representation method:
[0073] G Π= G n G d (18)
[0074]
[0075] in, G Π is the plane parameter expressed by the nearest point notation and is a three-dimensional vector, || G Π|| means G The modulus of Π, G n is the unit normal vector of the plane, Gd is the scale factor. Based on the plane extracted by AHC, the plane is filtered out by RANSAC and refitted. The parameters of the plane landmark can be restored by the plane extracted from the first frame:
[0076]
[0077] in, Represents the rotation matrix from the camera coordinate system to the world coordinate system, Indicates the position of the center of the world coordinate system in the camera coordinate system. is the unit normal vector of the plane in the camera coordinate system, Represents the scale factor of the plane in the camera coordinate system.
[0078] The observation equation of the plane feature is:
[0079]
[0080] in, Represents the plane parameters in the camera coordinate system using the closest point representation, which is a three-dimensional vector; is the rotation matrix from the world coordinate system to the camera coordinate system, G n is the unit normal vector of the plane in the world coordinate system, G d is the distance factor of the plane in the world coordinate system, Indicates the position of the center of the camera coordinate system in the world coordinate system. is the noise vector.
[0081] After linearizing it and stacking all observations, the residual equation for each plane feature can be expressed as:
[0082]
[0083] in, is the plane observation error; is the state error; is the plane road sign error; and is the Jacobian matrix of the plane observation relative to the state and error; n Π Indicates noise.
[0084] After obtaining the initial parameter estimates and observation equations, when there are sufficient observations, EKF updates are performed for the plane features. Due to the long-term existence of the plane, it is usually tracked for a long time but not continuously. All planes are considered as SLAM planes for optimization. Small, short-tracked planes will be delayed and marginalized after being lost for more than a certain period of time. Other planes will provide long-term constraints as permanent landmarks to overcome accumulated errors.
[0085] In step S104, a similarity matrix is determined based on the angle and distance between the two sets of planes, and it is determined based on the similarity matrix whether the observed plane has drift. For example, the similarity matrix includes a vertex similarity matrix and an edge similarity matrix, the vertex similarity matrix is determined based on the angle and distance between the two sets of planes, and the edge similarity matrix is determined based on the difference between the angle and distance between the two sets of planes. In step S105, in response to determining that the observed plane has drift, a previously created landmark plane without drift is fixed. In the example, after fixing the previously created landmark plane without drift, a pairwise equality constraint may also be enforced to update the landmark plane.
[0086] In the example, planes are organized into a graph structure to encode the geometric information of the scene. By matching the two graphs, overlapping and identical structures in the global map are searched, which are considered to be inconsistencies caused by drift. The errors are then corrected by aligning the detected structure "ghosts".
[0087] For example, the graph matching problem can be described as determining an assignment matrix X between them * :
[0088]
[0089] Among them, K P and K Q The similarity of nodes and edges is defined separately, X ij =1 means node i and node j match, X ij =0 means the two do not match. With the similarity matrix, the problem is solved by the FGM algorithm.
[0090] In the graph matching stage, planes are modeled as bounded patches, including properties such as id, normal, distance factor, convex hull point set, area, etc. In addition, angle, distance, category and overlap are used to describe the relationship between two plane patches. Based on these properties, the scene can be represented as a directed graph, whose vertices and edges are plane patches and their geometric relationships. Angles and distances are used to define the similarity matrix mentioned above, and other properties are used for subsequent verification. In particular, for vertices, the similarity matrix is defined as:
[0091]
[0092] Among them, θ ij represents the angle between two planes, d ij Represents the distance. For an edge, the similarity matrix is defined as:
[0093]
[0094] is a scoring function that maps variables to [0,1], Δθ, Δd represent the difference in angle and distance between two sets of planes, respectively. The proposed graph structure does not use the size of the plane patch and the distance to the center as comparison criteria, but considers the influence of partial observation problems in the incremental plane graph and fully explores the relative position relationship at the plane boundary to form directed edges.
[0095] The goal of drift detection is to search for similar and overlapping planar configurations from a global map, which is solved as a graph matching problem. Figure 4 As shown in Figure 1, first, the planes observed in the last ten frames are considered as the current active planes (local map), and their recent observations are constructed into a fully connected graph. Then, they are removed from the global map and matched with the remaining planes using the FGM algorithm using the similarity matrix defined above. For efficiency and robustness, a unary check is performed on the vertices to limit the number of candidate matches. The thresholds here are relatively weak constraints to avoid rejecting correct matches under large drifts. In addition, since FGM may encounter failure modes, the overlap is used to quantify the degree of overlap between two configurations, and a binary check is used to verify whether the matched edges have similar relative spatial positions. Due to changes in viewpoints, there may be occlusions between planes. Therefore, rather than requiring the edges to belong to the same category, their relative positions Δθ, Δd, and Δo are constrained to be within strict thresholds. If all constraints are met, the match is accepted and drift is considered to have occurred. Once drift is detected, the previously created landmark plane that is considered to be drift-free is fixed and pairwise equality constraints are enforced to update it:
[0096] z r = G Π d - G Π r +n r (26)
[0097] in, G Π d is the world coordinate system coordinate of the plane where the drift is detected; G Π r is the world coordinate of the unshifted plane, z r is the drift residual, n r is random noise. After that, the system state is optimized according to formula (22). Finally, the planes with high similarity will be merged and delayed marginalization.
[0098] The method according to the present disclosure makes full use of the spatial relationship of plane patches to detect the existence of drift, and corrects the error by performing drift removal updates. In the method according to the present disclosure, 3D points and plane features are effectively integrated into the polymorphic constrained Kalman function (MSCKF) framework. For point features, depth information is used to improve the accuracy of point triangulation and is formulated as a measurement value for updating the system state. For plane features, they are added to the state vector as direct observations. And through the proposed drift detection strategy, long-term navigation can be promoted for searching for overlapping and identical structures in a plane map, thereby suppressing cumulative drift. This method can cope with larger drifts and enhance the performance of the odometer system under long-term positioning.
[0099] Figure 2 is a block diagram illustrating the workflow of a planar-assisted visual-inertial odometry according to an example of the present disclosure.
[0100] The workflow of the planar-assisted visual-inertial odometry according to the example of the present disclosure mainly includes four parts: inertial measurement unit (IMU) integration and propagation, feature detection and association, extended Kalman filter (EKF) update and error suppression.
[0101] like Figure 2 As shown, the workflow of the plane-assisted visual-inertial odometry according to the example of the present disclosure includes: first taking color images, depth images and acquiring IMU data, and performing spatiotemporal alignment; given the input RGB-D and inertial sequences (i.e., gyroscope and accelerometer data), first applying IMU measurements to propagate system states and covariances; after state initialization, points and planes are detected and associated in parallel, and updated during the EKF update process; if necessary, drift is detected according to a graph matching strategy and suppressed by de-drifting updates; thereafter, small and short planes are delayed marginalized after being lost for a period of time.
[0102] Figure 3 Accurate trajectory and consistent floor plan results obtained using a drift suppression strategy according to the present disclosure are shown. Figure 4 are diagrams showing different positional relationships between planar patches.
[0103] like Figure 3 As shown, thanks to the drift suppression strategy, the system can detect overlapping and identical configurations in the floor plan and align them to cope with the accumulated errors, resulting in accurate trajectories and more consistent floor plans. Figure 4The different relative positions of two planar patches are shown when viewed from the common perpendicular direction of their normal vectors. In each figure, the dot on plane i represents the point on plane i that is closest to plane j, and the corresponding line represents the distance; the dot on plane j and the corresponding line measure the distance from plane j to plane i, and the line on plane j marks their overlapping area, and negative numbers represent parallel distances in (d) and (f).
[0104] Figure 5 is a diagram illustrating an error suppression flow according to an example of the present disclosure.
[0105] Reference Figure 5 , the process of detecting drift includes: first, the planes observed in the last ten frames are regarded as the current active planes (local map), and their recent observations are constructed into a fully connected graph; then, the active planes are removed from the global map and matched with the remaining planes (i.e., the planes remaining after the active planes are removed from the map) using the FGM algorithm with the similarity matrix defined above. For efficiency and robustness, unary checks are performed on the vertices to limit the number of candidate matches, and unmatched planes are filtered by comparing the angle and distance of the planes in the world coordinate system. The angle and distance thresholds here are relatively weak constraints to avoid rejecting correct matches under large drift. In addition, since FGM may encounter failure modes, overlap is used to quantify the degree of overlap between two configurations, and a binary check is used to verify whether the matched edges have similar relative spatial positions. Due to changes in viewpoint, there may be occlusions between planes. Therefore, edges are not required to belong to the same category, but their relative positions Δθ, Δd, and Δo are restricted to be within strict thresholds (e.g., Figure 4 Due to occlusion and noise, there may be matches between plane groups of different categories. If all constraints are met, the match is accepted and drift is considered to have occurred. Once drift is detected, the previously created landmark planes that are considered to be drift-free are fixed and updated by enforcing pairwise equality constraints.
[0106] Figure 6 A visualization of the localization trajectory on the robot sequence is shown. Figure 7 Shown are floor plans and trajectory visualizations constructed on a long corridor sequence.
[0107] Tables 1 and 2 show the root mean square errors of the absolute position errors of the trajectories when the general method and the method according to the present disclosure are adopted. Figure 6 and Figure 7 The corresponding visualization results are shown. Figure 6 In , the horizontal and vertical axes are the XY coordinates of the camera, respectively. Whether it is a long corridor sequence or a small room sequence, it can achieve high robustness and high accuracy. Figure 7 In the figure, PGD w / o P represents the proposed system with the plane module turned off, PGD w / o G represents the proposed system with the error suppression module turned off, and PGD-VIO represents the proposed complete system, which shows the mapping and positioning results on a 210-meter corridor. The two figures on the left show the plane map without drift suppression and the plane map with drift suppression added, respectively; the rightmost figure shows the comparison trajectory, where the first 500 frames are used to align the trajectory with the ground truth, and the endpoint marks reflect their cumulative drift. Before the first drift is detected, the two are completely overlapped, gradually increasing over time. The system according to the present disclosure effectively reduces the final cumulative error and enhances the consistency of the final map.
[0108] Table 1
[0109]
[0110]
[0111] Table 2
[0112] sequence name General approach Methods according to the present disclosure corridor1 1.039m 0.192m corridor2 1.639m 0.075m corridor3 0.435m 0.404m corridor4 0.706m 0.121m
[0113] Figure 8 1 is a block diagram showing an error suppression device for a plane-assisted visual-inertial odometry according to an exemplary embodiment of the present disclosure. The plane-assisted visual-inertial odometry includes an inertial measurement unit and an RGB-D camera.
[0114] like Figure 8 As shown, the error suppression device 800 for a plane-assisted visual-inertial odometer according to an exemplary embodiment of the present disclosure includes: an integration and propagation unit 801, configured to determine a state vector of an inertial measurement unit based on the measurement values of a local rotational velocity and a local translational acceleration obtained by the inertial measurement unit; a feature detection and association unit 802, configured to determine point features and plane features based on a color map and a depth map obtained by an RGB-D camera; an extended Kalman filter update unit 803, configured to perform an update on the point features and plane features through an extended Kalman filter determined based on the state vector; a drift detection unit 804, configured to determine a similarity matrix based on the angle and distance between two groups of planes, and determine whether the observed plane drifts based on the similarity matrix; an error suppression unit 805, configured to fix a previously created drift-free landmark plane in response to determining that the observed plane drifts.
[0115] In the example, the feature detection and association unit 802 is configured to: for color images, use the FAST corner detection algorithm to extract feature points, and use the sparse optical flow tracking method to track the feature points; for depth images, use the agglomeration hierarchical clustering algorithm to extract plane features, and associate them with the planes in the map based on the plane normal and distance factor.
[0116] In the example, the extended Kalman filter update unit 803 is configured to: classify point features into synchronous positioning and mapping point features and polymorphic constrained Kalman function point features according to the tracked length; for the synchronous positioning and mapping point features, add them to the state vector and update their coordinates; for the polymorphic constrained Kalman function point features, use the null space projection to eliminate the dependence on them before updating them.
[0117] In the example, the extended Kalman filter updating unit 803 is configured to update all the plane features as simultaneous positioning and mapping plane features.
[0118] In an example, the similarity matrix includes a vertex similarity matrix and an edge similarity matrix, the vertex similarity matrix is determined based on the angle and distance between two sets of planes, and the edge similarity matrix is determined based on the difference between the angle and distance between the two sets of planes.
[0119] In an example, the error suppression unit 805 is further configured to enforce pairwise equality constraints to update the landmark plane after fixing the previously created drift-free landmark plane.
[0120] Combination of the above Figures 1 to 7 The specific operations shown are respectively Figure 8 The error suppression device 800 for plane-assisted visual-inertial odometer shown in the figure is executed by corresponding units, and the specific operation details will not be repeated here.
[0121] Fig. 9 is a block diagram illustrating a computing system including at least one computing device and at least one storage device storing instructions according to an exemplary embodiment of the present disclosure.
[0122] like Fig. 9 As shown, a computing system 900 provided according to an exemplary embodiment of the present invention includes a computing device 901 and a storage device 902, wherein the storage device 902 stores computer executable instructions. When the computer executable instructions are executed by the computing device 901, the error suppression method for a plane-assisted visual-inertial odometer described in any of the aforementioned embodiments is executed.
[0123] The computing device 901 is deployed in a server or client, and may also be deployed on a node device in a distributed network environment. In addition, the computing device 901 may be a PC computer, a tablet device, a personal digital assistant, a smart phone, a web application or other device capable of executing the above-mentioned instruction set. Here, the computing device is not necessarily a single computing device, but may also be any device or circuit capable of executing the above-mentioned instruction (or instruction set) individually or jointly. The computing device may also be a part of an integrated control system or a system manager, or may be configured as a portable electronic device interconnected with an interface locally or remotely (e.g., via wireless transmission). In the computing device, the processor includes a central processing unit (CPU), a graphics processing unit (GPU), a programmable logic device, a dedicated processor system, a microcontroller or a microprocessor. As an example and not limitation, the processor also includes an analog processor, a digital processor, a microprocessor, a multi-core processor, a processor array, a network processor, etc.
[0124] According to another aspect of the present disclosure, a plane-assisted visual-inertial odometry system is provided, comprising: an inertial measurement unit; an RGB-D camera; and a controller, configured to execute the error suppression method for plane-assisted visual-inertial odometry described in any of the aforementioned embodiments.
[0125] According to another aspect of the present disclosure, a computer-readable storage medium storing instructions is provided, which, when executed by at least one computing device, causes the at least one computing device to execute the error suppression method for a plane-assisted visual-inertial odometer described in any of the aforementioned embodiments. Computer-readable storage media include magnetic media such as floppy disks and tapes, optical media (including compact disk (CD) ROMs and DVD ROMs), magneto-optical media such as floppy disks, hardware devices such as ROMs, RAMs, and flash memories designed to store and execute program commands. The instructions may include language codes executable by a computer using an interpreter and machine language codes generated by a compiler.
[0126] By adopting the present disclosure, it is possible to achieve: based on the original multi-state constrained Kalman function (MSCKF) framework, effectively integrate depth information and plane measurement, use the depth of the point to improve the feature triangulation result, and use CP parameterized plane measurement and depth measurement to be designed and integrated into the EKF update process; make full use of the different spatial relationships of plane patches, and apply a graph-based strategy for drift suppression. As a result, the system's performance in coping with cumulative drift (especially in long-term navigation) can be improved. For indoor structured areas, more geometric metric information can be provided, which can be used to further constrain motion. Due to its extensive spatial and temporal observability, it can effectively reduce system drift.
[0127] The processes, methods or algorithms disclosed herein may be transmitted to or implemented by a processing device, a controller or a computer, which may include any existing programmable electronic control unit or a dedicated electronic control unit. Similarly, the processes, methods or algorithms may be stored in a variety of forms as data and instructions that can be executed by a controller or a computer, including but not limited to information being permanently stored on a non-writable storage medium (such as a ROM device) and information being variably stored on a writable storage medium (such as a floppy disk, a tape, a CD, a RAM device, and other magnetic and optical media). The processes, methods or algorithms may also be implemented in a software executable object. Optionally, the processes, methods or algorithms may be implemented in whole or in part using suitable hardware components (such as an ASIC, an FPGA, a state machine, a controller or other hardware components or devices) or a combination of hardware components, software components and firmware components.
[0128] Although the present disclosure includes specific examples, it will be apparent to those of ordinary skill in the art that various changes in form and detail may be made in these examples without departing from the spirit and scope of the claims and their equivalents. The examples described herein will be considered to be descriptive only and not for limiting purposes. The description of the features or aspects in each example will be considered to be applicable to similar features or aspects in other examples. Suitable results may be obtained if the described techniques are performed in a different order, and / or if the components in the described systems, architectures, devices, or circuits are combined in different ways and / or replaced or supplemented with other components or their equivalents in the described systems, architectures, devices, or circuits. Therefore, the scope of the present disclosure is not limited by specific embodiments, but by the claims and their equivalents, and all variations within the scope of the claims and their equivalents will be interpreted as included in the present disclosure.
Claims
1. An error suppression method for a planar assisted visual-inertial odometry, characterized in that: The planar-assisted visual-inertial odometry includes an inertial measurement unit and an RGB-D camera, and the error suppression method includes: Determining a state vector of the inertial measurement unit based on measurements of a local rotational velocity and a local translational acceleration obtained by the inertial measurement unit; Determine point features and plane features based on the color image and depth image obtained by the RGB-D camera; Performing an update on the point features and the plane features by using an extended Kalman filter determined based on the state vector; Determine a similarity matrix based on the angle and distance between the two sets of planes, and determine whether the observed planes drift based on the similarity matrix; In response to determining that the observed plane has drift, a previously created landmark plane without drift is fixed.
2. The error suppression method according to claim 1, characterized in that: The steps to determine point features and plane features include: For color images, the FAST corner detection algorithm is used to extract feature points, and the sparse optical flow tracking method is used to track the feature points; For the depth map, an agglomerative hierarchical clustering algorithm is used to extract plane features and associate them with planes in the map based on the plane normal and distance factor.
3. The error suppression method according to claim 1, characterized in that: The step of performing an update on the point feature comprises: Point features are classified into simultaneous positioning and mapping point features and polymorphic constrained Kalman function point features according to the length of the tracked point features; For the synchronous positioning and mapping point features, add them to the state vector and update their coordinates; For the point features of polymorphic constrained Kalman function, the null space projection is used to eliminate the dependence on it before updating.
4. The error suppression method according to claim 1 or 3, characterized in that: The step of performing an update on the plane feature comprises: Update all planar features as simultaneous positioning and mapping planar features.
5. The error suppression method according to claim 1, characterized in that: The similarity matrix includes a vertex similarity matrix and an edge similarity matrix. The vertex similarity matrix is determined based on the angle and distance between two groups of planes, and the edge similarity matrix is determined based on the difference between the angle and distance between the two groups of planes.
6. The error suppression method according to claim 5, characterized in that: The error suppression method further includes updating the landmark plane by enforcing pairwise equality constraints after fixing the previously created drift-free landmark plane.
7. An error suppression device for a planar assisted visual-inertial odometer, characterized in that The planar assisted visual-inertial odometer includes an inertial measurement unit and an RGB-D camera, and the error suppression device includes: an integration and propagation unit configured to determine a state vector of the inertial measurement unit based on the measurement values of the local rotational velocity and the local translational acceleration obtained by the inertial measurement unit; a feature detection and association unit, configured to determine point features and plane features based on a color image and a depth image obtained by an RGB-D camera; An extended Kalman filter updating unit configured to perform an update on the point feature and the plane feature through an extended Kalman filter determined based on the state vector; a drift detection unit configured to determine a similarity matrix based on the angle and distance between the two sets of planes, and to determine whether the observed planes drift based on the similarity matrix; The error suppression unit is configured to fix a previously created landmark plane without drift in response to determining that the observed plane has drift.
8. A planar assisted visual-inertial odometry system, characterized in that: include: Inertial measurement unit; RGB-D camera; as well as A controller is configured to execute the error suppression method for a plane-assisted visual-inertial odometry according to any one of claims 1-6.
9. A computing system comprising at least one computing device and at least one storage device storing instructions, characterized in that: When the instructions are executed by the at least one computing device, the at least one computing device is prompted to perform the error suppression method for planar-assisted visual-inertial odometry according to any one of claims 1-6.
10. A computer-readable storage medium storing instructions, characterized in that: When the instructions are executed by at least one computing device, the at least one computing device is prompted to perform the error suppression method for a plane-assisted visual-inertial odometry according to any one of claims 1-6.
