CN111815684B - Space multivariate feature registration optimization method and device based on unified residual error model - Google Patents
Space multivariate feature registration optimization method and device based on unified residual error model Download PDFInfo
- Publication number
- CN111815684B CN111815684B CN202010537788.5A CN202010537788A CN111815684B CN 111815684 B CN111815684 B CN 111815684B CN 202010537788 A CN202010537788 A CN 202010537788A CN 111815684 B CN111815684 B CN 111815684B
- Authority
- CN
- China
- Prior art keywords
- global
- current frame
- local
- matching
- residual error
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
Landscapes
- Engineering & Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Image Analysis (AREA)
Abstract
The invention relates to a space multi-feature registration optimization method and device based on a unified residual error model. And in the back-end local optimization link, a local optimization equation is defined according to the unified residual error model, and joint optimization of three types of characteristic elements, namely points, lines and surfaces in the local map under the unified optimization equation is realized. The method expresses different types of parameters in the same parameterization mode, defines the same residual error model, and ensures the stability, efficiency and precision of map optimization.
Description
Technical Field
The invention relates to the field of photogrammetry and computer vision, in particular to a space multivariate feature registration optimization method and device based on a unified residual error model.
Background
In the related fields of automatic driving navigation, high-precision maps and the like, the SLAM technology is always a research hotspot, the realization of the current algorithm is mature, and particularly, the visual SLAM technology based on feature points calculates and obtains the spatial coordinates of the feature points under a local coordinate system through feature extraction, tracking and multi-view intersection at the front end, so that real-time registration tracking and local optimization are carried out according to the spatial three-dimensional points, the full flow of synchronous positioning and map building is completed, and the visual SLAM technology is one of the mainstream algorithms in the field of the current visual SLAM.
A three-dimensional point map constructed based on point features cannot effectively express a real scene structure, and meanwhile, the data volume of the feature point map is too large, so that a large amount of information redundancy exists. Therefore, the SLAM algorithm based on the line feature and the surface feature is gradually paid attention to by researchers, particularly in the environment with many artificial buildings such as streets, indoor buildings and the like, the line feature and the surface feature can reflect the structural feature of a road scene relative to the feature point, the map data volume can be greatly compressed by using the line feature and the surface feature extracted from the point cloud to represent the point feature, and the influence of factors such as environmental illumination and the like can be resisted in tracking. Therefore, the visual SLAM algorithm based on line and surface features begins to become a core technology of vehicle mapping.
However, due to the diversity of scenes, in some special scenes, the line features and the area features are not more capable of reflecting the structural features of the scenes than the point features. In addition, the traditional SLAM algorithm based on point, line and surface features usually extracts feature points, structural lines and surface features on an image, acquires feature point clouds according to depth information, performs feature matching and tracking, establishes an observation error equation, and calculates the optimal robot position and the optimal surrounding environment features by solving the minimum observation error value through linear or nonlinear optimization. Different parametric expression modes are pertinently adopted for modeling each different types of feature point clouds, different residual equations are constructed for registration and optimization, the registration algorithm is still based on the expansion of the traditional ICP algorithm, and meanwhile, different residual models and registration strategies are adopted for different types of features, so that the instability, efficiency and precision of the optimization model are reduced.
The noun explains:
reference frame: the frame preferentially matched with the current frame is generally a frame which is successfully tracked in the previous round or a frame with the most matching characteristic number in a local map;
and (3) common view frame: matching (observing) the frame with the current frame to the same road sign;
global signposts: and the object is composed of points, lines and surface features under the world coordinate system.
Disclosure of Invention
The invention provides a space multi-feature registration optimization method and device based on a unified residual error model, aiming at the technical problems in the prior art, the method provides a method for constructing a unified parametric expression model for feature point clouds of three different types, namely point, line and surface, and the unified residual error model is designed according to the feature matching relations of the different types in a front-end matching and tracking link for fusion registration. And in the rear-end local optimization link, a local optimization equation is defined according to the unified residual error model, and joint optimization of three types of characteristic elements, namely points, lines and surfaces in the local map under the unified optimization equation is realized.
The technical scheme for solving the technical problems is as follows:
in a first aspect, the invention provides a unified residual error model-based space multivariate feature registration optimization method, which comprises the following steps:
carrying out uniform parametric description on the characteristics of points, lines and surfaces in the single-frame image;
converting the local characteristic element m in the current frame into a global coordinate system according to the initial pose X of the current frame in the global coordinate system to obtain a global parameter m' of the local characteristic element m; the global coordinate system is a starting frame camera coordinate system;
taking the global parameter m 'as an input node, finding a global landmark which is spatially nearest to the global parameter m' by using a KDTree search algorithm in a reference frame, a local map and a global map, and establishing a matching relation between the local feature element m of the current frame and the global landmark;
defining a uniform residual error model, calculating a matching residual error between a local feature element m of the current frame and the global landmark according to a matching relation, and iteratively solving the optimal pose of the current frame under a global coordinate system by adopting a Gauss-Newton method according to the matching residual error;
and carrying out image optimization on the local map by using the optimal pose of each frame in the local map under the global coordinate system.
Further, the uniform parameterization description of the point, line and surface features in the single-frame image includes:
defining three-dimensional center point p' of point p, line L and plane alpha characteristic and direction vector
3 × 3 attitude matrix R, 3 × 3 morphology matrix Ω;
wherein the three-dimensional center point p' of the point p, the line L, the plane alpha characteristic, and the direction vector
As follows:
the 3 x 3 attitude matrix R of the points p, lines L, faces α features is as follows:
the 3 x 3 morphology matrix Ω of the points p, lines L, faces α features is as follows:
| type of feature | Form matrix omega |
| Point p | diag(1,1,1) |
| Line L | diag(0,1,1) |
| Face alpha | diag(1,0,0) |
Based on the above definition, the characteristics of the arbitrary point p, the line L, and the plane α are parameterized as: m: { p' m ,R m ,Ω m }。
Further, the converting the local feature element m in the current frame to the global coordinate system according to the initial pose X of the current frame in the global coordinate system to obtain a global parameter m' of the local feature element m includes:
acquiring a pose initial value X of a current frame under a global coordinate system; definition X ═ { R X |t X E is SE3, wherein SE3 is a three-dimensional special Euclidean group, R X Is represented asCorresponding 3 x 3 dimensional attitude rotation matrix of the previous frame under the global coordinate system, t X Representing a corresponding 3 x 1 dimensional pose translation matrix of the current frame under a global coordinate system;
according to the formula
And converting the local feature element m contained in the current frame into a global coordinate system to obtain a global parameter m'.
Further, the step of using the global parameter m 'as an input node, finding a global landmark which is spatially nearest to the global parameter m' in the reference frame, the local map and the global map by using a KDTree search algorithm, and establishing a matching relationship between the current frame local feature element m and the global landmark includes:
parameterizing the global landmark into space nodes according to the feature type of the global landmark and storing the space nodes as a binary tree structure;
taking the frame with the maximum matching amount in the previous tracking success or local map matching as a reference frame, and matching the local feature elements in the current frame with the global landmarks in the reference frame;
updating a local map by taking the current frame as a reference, and matching local characteristic elements in the current frame with global landmarks stored in the local map;
and if the matching of the global feature elements in the current frame with the signposts in the reference frame and the matching of the local feature elements in the current frame with the signposts stored in the local map fail, performing global search and matching the current frame with the global signposts in the global map.
Further, the method for finding the global landmark spatially nearest to the global parameter m' by using the KDTree search algorithm in the reference frame, the local map, and the global map and establishing the matching relationship between the local feature element m of the current frame and the global landmark further includes: and if the feature matching of the current frame, the reference frame, the local map and the global map is not successful, adding the global parameter m' of the local feature element m into the global map as a new global landmark.
Further, the unified residual error model is defined as follows:
defining the jth local feature element m of the ith frame ij :{p' ij ,R ij ,Ω ij With global signpost
7-dimensional residual model in between:
wherein, the first and the second end of the pipe are connected with each other,
is the ith frame attitude matrix, SE3 is a three-dimensional special Euclidean group,
represents the 3 x 3 dimensional attitude rotation matrix of the ith frame in the global coordinate system, t i Representing a 3 x 1 dimensional pose translation matrix corresponding to the ith frame in the global coordinate system;
is the three-dimensional central point of the global landmark,
is the direction vector of the global signpost.
Further, the calculating a matching residual between the local feature element m of the current frame and the global landmark according to the matching relationship, and iteratively solving the optimal pose of the current frame in the global coordinate system by using a gauss-newton method according to the matching residual, includes:
301, interpolating the pose of the current frame according to the information of the exogenous sensor or predicting the pose of the current frame based on the historical frame according to the uniform motion model
Step 302, calculating a matching residual error of each local feature element of the current frame according to the pose of the current frame according to a residual error model; defining the matching residual error of the kth local feature element as e k Calculating the matching residual of all local feature elements of the current frame according to the following formula
Step 303, judge
Whether the iteration number is not reduced or the iteration number is maximum after the iteration is updated relative to the previous round, if so, the iteration is ended and the current iteration number is selected
As the optimal pose of the current frame, otherwise, executing step 304;
step 304, traversing the matching residual error of each local feature element of the current frame, and defining the Jacobian matrix of the relative state quantity of the k-th residual error as J k Calculating a Jacobian matrix of the relative state quantity of each element residual according to the following formula;
step 305, respectively calculating an intermediate variable matrix: h matrix and b matrix, the calculation formula is as follows:
wherein omega k Is a residual error information matrix;
step 306, calculating the disturbance quantity delta of pose iterative updatex：Δx←-H -1 b；
Step 307, updating the predicted pose of the current frame by the disturbance amount deltax according to the following formula
And jumping to step 302;
wherein R is x ()、R y ()、R z () Representing the Rodriguz transformation, Δ α, rotated by a certain angle around the x, y, z axes of the local coordinate system, respectively x ，Δα y ，Δα z Is the relative rotation angle around the x, y, z axis of the local coordinate system.
In a second aspect, the present invention provides a device for optimizing spatial multivariate feature registration based on a unified residual error model, including:
the parameter description module is used for carrying out uniform parametric description on the characteristics of points, lines and surfaces in the single-frame image;
the matching module is used for converting the local characteristic element m in the current frame into a global coordinate system according to the initial pose X of the current frame in the global coordinate system to obtain a global parameter m' of the local characteristic element m; the global feature element m 'is used as an input node, a global landmark which is spatially nearest to the global parameter m' is found in a reference frame, a local map and a global map by using a KDTree search algorithm, and a matching relation between the local feature element m of the current frame and the global landmark is established;
the registration module is used for defining a uniform residual error model, calculating a matching residual error between the local feature elements of the current frame and the global landmark according to the matching relation, and iteratively solving the optimal pose of the current frame under a global coordinate system by adopting a Gauss-Newton method according to the matching residual error;
and the optimization module is used for carrying out map optimization on the local map by using the optimal pose of each frame in the local map under the global coordinate system.
In a third aspect, the present invention provides an electronic device comprising:
a memory for storing a computer software program;
and the processor is used for reading and executing the computer software program stored in the memory, so as to realize the space multivariate feature registration optimization method based on the unified residual error model in the first aspect of the invention.
In a fourth aspect, the present invention provides a non-transitory computer-readable storage medium, in which a computer software program for implementing the unified residual error model-based spatial multivariate feature registration optimization method according to the first aspect of the present invention is stored.
The invention has the beneficial effects that:
1. different parametric expression models are required to be constructed for the extracted feature point clouds of different types of points, lines and surfaces, a uniform parametric model is defined, and three different types of primitive features of the points, the lines and the surfaces can be expressed in a parameter form.
2. Given a group of well matched point, line and surface characteristics, different registration methods need to be designed for different types of characteristics, and the invention provides a uniform residual error model for all types of element characteristics according to a uniform characteristic parameterization form to carry out fusion registration.
3. When a local optimization equation is constructed, different residual error models and optimization functions are required to be defined for three different types of characteristics, namely points, lines and surfaces, for optimization.
Drawings
Fig. 1 is a flowchart of a spatial multivariate feature registration optimization method based on a unified residual error model according to an embodiment of the present invention;
FIG. 2 is a flow chart of a local map update provided by an embodiment of the present invention;
FIG. 3 is a flow chart of feature matching provided by an embodiment of the present invention;
fig. 4 is a local optimization factor graph provided by the embodiment of the present invention.
Detailed Description
The principles and features of this invention are described below in conjunction with the following drawings, which are set forth by way of illustration only and are not intended to limit the scope of the invention.
Example 1
As shown in fig. 1, an embodiment of the present invention provides a method for optimizing spatial multivariate feature registration based on a unified residual error model, including the following steps:
carrying out uniform parametric description on the characteristics of points, lines and surfaces in the single-frame image;
converting the local characteristic element m in the current frame into a global coordinate system according to the initial pose X of the current frame in the global coordinate system to obtain a global parameter m' of the local characteristic element m; the global coordinate system is a starting frame camera coordinate system;
taking the global parameter m 'as an input node, finding a global landmark which is spatially nearest to the global parameter m' by using a KDTree search algorithm in a reference frame, a local map and a global map, and establishing a matching relation between the local feature element m of the current frame and the global landmark;
defining a uniform residual error model, calculating a matching residual error between the local feature elements of the current frame and the global landmark according to a matching relation, and iteratively solving the optimal pose of the current frame under a global coordinate system by adopting a Gauss-Newton method according to the matching residual error;
and carrying out image optimization on the local map by using the optimal pose of each frame in the local map under the global coordinate system.
Specifically, the method comprises the following steps:
s100, uniformly parameterizing point, line and surface characteristics
And after acquiring the point cloud of the point p, the line L and the surface alpha extracted from the single-frame image, carrying out uniform parameterization representation on the characteristic elements. Any characteristics of points, lines and surfaces in space can be expressed according to the formula (1.1),
(p-p') T RΩR T (p-p')＝0 (1.1)
wherein p' is the 3-dimensional center point of the feature, R is the 3 x 3 attitude matrix, and Ω is the 3 x 3 morphology matrix.
Therefore, the arbitrary point p, line L, and plane α object can be expressed as expression (1.2) based on expression (1.1).
m:{p' m ,R m ,Ω m } (1.2)
Table 1.1 shows the center point p' and the direction vector of the point p, the line L, the plane alpha feature
The manner of definition of (1). The attitude matrix R is calculated according to the direction vector, and the form matrix omega is defined according to the types of the three elements of the point, the line and the surface. The posture matrix R and the form matrix Ω of the point p, the line L, the plane α feature are defined as shown in tables 1.2 and 1.3.
TABLE 1.1 center point p' and direction vector of multivariate feature element
Definition of
TABLE 1.2 attitude matrix R definition of multivariate feature elements
TABLE 1.3 morphological matrix omega definition of multivariate feature elements
| Type of feature | Form matrix omega |
| Point p | diag(1,1,1) |
| Line L | diag(0,1,1) |
| Face alpha | diag(1,0,0) |
S200, feature matching
Firstly, predicting an initial pose value of a current frame in a global coordinate system, taking a camera coordinate system of an initial frame as the global coordinate system, performing pose interpolation on the current frame according to information of exogenous sensors such as IMU (inertial measurement Unit) or estimating an initial pose value X of 4-dimension of the current frame in the global coordinate system according to relative motion of the previous two frames according to a uniform motion model under the condition that no exogenous sensor exists, and defining X to be { R to be R X |t X E is SE3, wherein SE3 is a three-dimensional Special Euclidean Group (Special Euclidean Group) satisfying
R X Representing a 3-x 3-dimensional pose rotation matrix in a global coordinate system and satisfying R X Is epsilon of SO3, wherein SO3 is a three-dimensional Special orthogonal group (Special orthogonal group) satisfying
t X Representing a 3 x 1 dimensional pose translation matrix under a global coordinate system, and satisfying
After the current frame creates a local feature element m under a camera coordinate system according to the formula (1.1) and the formula (1.2), the m is converted into a global coordinate system according to the pose initial value X according to the formula (1.3), and m' is obtained.
And matching m 'with the global landmark, and if the global landmark does not exist or is not matched with the global landmark, adding m' as a new global landmark into the global map. In order to accelerate the matching process, the matching of m' and the global signposts is carried out based on a KDTree searching mode, and the global signposts are parameterized into space nodes according to the element types of the global signposts as shown in a table 1.4 and are stored into a binary tree structure. According to the type of the local feature elements, m' is parameterized into nodes to be searched as shown in table 1.4, and the nodes are used as input parameters to carry out nearest neighbor search in the spatial binary tree, so that the matched global signposts are obtained.
TABLE 1.4 multivariate feature factor KDTree node parameter model
To improve the matching efficiency, matching is preferentially performed in the reference frame and the local map. Fig. 3 shows a KDTree-based matching process, which includes the steps of:
s201, taking the frame with the maximum matching amount in the previous tracking success or local map matching as a global reference frame, and matching the current frame element with the observation landmark of the reference frame.
S202: in order to ensure that the current frame matches global landmarks as much as possible, the local map is updated by taking the current frame as a reference, and then elements of the current frame are matched with landmarks stored in the local map.
S203: and if the global landmark is not matched in the steps S201 and S202, performing global search, and matching the current frame with the landmark of the global map.
The local map update process shown in fig. 2 includes:
(1) extracting frames with common observation characteristics with the current frame to form a common-view frame set;
(2) extracting frames which are adjacent to the common-view frame and have no common observation characteristic with the current frame to form a field frame set;
(3) the common-view frame and the field frame jointly form a local frame set, and all the signposts matched with the local frame are obtained to form a local signpost set;
(4) the local signposts and the local frames together form a local map.
Step 300, single frame matching
Defining the jth local feature element m of the ith frame ij :{p' ij ,R ij ,Ω ij And global signpost
7-dimensional residual model in between:
wherein the content of the first and second substances,
is an ith frame position and posture matrix,
is the direction vector of the global landmark.
The residual covariance matrix is defined as shown in equation (1.5).
The definition of the covariance matrix (information matrix) for matching residuals between different feature types is shown in table 1.5.
TABLE 1.5 residual information matrix definition
And according to the matching residual error model of the current frame and the global landmark, iteratively solving the optimal pose of the current frame under the global coordinate system by adopting a Gauss-Newton method. The algorithm comprises the following steps:
s301, performing pose interpolation on the current frame according to the information of the external sensor or predicting the pose of the current frame based on the historical frame according to the uniform motion model. The embodiment predicts the initial pose of the current frame based on the historical frame according to the uniform motion model
S302, according to the formula (1.4), predicting the pose of the current frame
And calculating the matching residual error of each matching element of the current frame. Defining the matching residual of the kth element as e k Calculating the matching residual error of all local feature elements of the current frame according to the formula (1.6)
If it is not
After the iteration is updated relatively to the previous round, the iteration is not reduced or the maximum iteration number is reached, the iteration is ended, and the current iteration number is taken
And if not, the step S303 is carried out.
S303, traversing the matching residual of each element of the current frame, and defining the Jacobian matrix of the k-th residual relative state quantity as J k Then, a Jacobian matrix of the relative state quantities of the residuals of the respective elements is calculated as shown in equation (1.7).
S304, respectively calculating the intermediate variable matrixes H according to the forms of the formula (1.8) and the formula (1.9)Matrix, b matrix. Wherein omega k Is a residual information matrix.
And S305, calculating the disturbance quantity delta x of the pose iterative update according to the formula (1.10).
Δx←-H -1 b (1.10)
S306: updating the predicted pose of the current frame by using the disturbance quantity delta x
Go back to step 3.2.
Δ x is a 6 × 1 dimensional perturbation quantity expressed in the form:
where Δ t is the translational component and Δ α is the rotational component. In order to apply a disturbance quantity Deltax to
First, Δ X is converted into the form Δ X of a rotation matrix and a translation vector by a v2t () function according to equation (1.12).
Wherein R is x ()，R y ()，R z () Representing the Rodriguz transformation, Δ α, rotated by a certain angle around the x, y, z axes of the local coordinate system, respectively x ，Δα y ，Δα z Is the relative rotation angle around the x, y, z axis of the local coordinate system.
Left-multiplying Δ X according to equation (1.13)
Implementing a current frame
Iterative prediction of (2).
S400, local map optimization
And by using a nonlinear least square method and iteratively correcting the poses of the signposts and the observation frames in the local map, the aim of overall minimization of the matching residual errors is fulfilled. And updating the local map elements where the current frame is located according to the method shown in fig. 3, wherein the local map elements comprise the local frame and the local signpost elements. Setting a local frame to be optimized as the first N frames with the most common-view characteristic number with the current frame, setting other frames with the common-view characteristic number with the local frame as fixed frames in optimization, customizing corresponding graph optimization vertexes and edges by utilizing a graph optimization algorithm, setting the vertexes of each edge as a global landmark pose and an observation frame pose to be optimized, setting a matching residual error item of the corresponding frame and the global landmark of each edge as defined by a formula (1.4). The iterative optimization process of each optimization edge is performed according to the gauss-newton method described in steps S301 to S306. The local optimization factor is shown in fig. 4, where the circle node represents the system state to be estimated, and the box connected to the box node represents the observation or a priori, including a constraint on the variable to which it is connected.
The optimized state variable χ comprises N poses and M landmark parameters χ: { M 1 ,m 2 ,...,m M ,X 1 ,X 2 ,...,X N In which X is i Representing the ith frame attitude matrix, m j Representing the jth global landmark parameter. The corresponding optimized function is shown as formulas (1.14) and (1.15), wherein e ij Such as
Formula 1.4, omega ij The definition of (A) is shown in formula (1.5). Chi shape * Refers to the optimal variables that satisfy the minimization.
Example 2
In a second aspect, the present invention provides a device for optimizing spatial multivariate feature registration based on a unified residual error model, including:
the parameter description module is used for carrying out uniform parametric description on the characteristics of points, lines and surfaces in the single-frame image;
the matching module is used for converting the local characteristic element m in the current frame into a global coordinate system according to the initial pose X of the current frame in the global coordinate system to obtain a global parameter m' of the local characteristic element m; the global feature element m 'is used as an input node, a global landmark which is spatially nearest to the global parameter m' is found in a reference frame, a local map and a global map by using a KDTree search algorithm, and a matching relation between the local feature element m of the current frame and the global landmark is established;
the registration module is used for defining a uniform residual error model, calculating a matching residual error between the local feature elements of the current frame and the global landmark according to the matching relation, and iteratively solving the optimal pose of the current frame under a global coordinate system by adopting a Gauss-Newton method according to the matching residual error;
and the optimization module is used for carrying out map optimization on the local map by using the optimal pose of each frame in the local map under the global coordinate system.
Example 3
The present invention provides an electronic device including:
a memory for storing a computer software program;
and the processor is used for reading and executing the computer software program stored in the memory, so as to realize the space multivariate feature registration optimization method based on the unified residual error model in the first aspect of the invention.
It should also be noted that the logic instructions in the computer software program can be implemented in the form of software functional units and stored in a computer readable storage medium when the software functional units are sold or used as independent products. Based on such understanding, the technical solutions of the embodiments of the present invention may be essentially implemented or make a contribution to the prior art, or may be implemented in the form of a software product stored in a storage medium and including instructions for causing a computer device (which may be a personal computer, a server, or a network device) to execute all or part of the steps of the methods described in the embodiments of the present invention. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents, improvements and the like that fall within the spirit and principle of the present invention are intended to be included therein.
Claims (7)
1. A space multivariate feature registration optimization method based on a unified residual error model is characterized by comprising the following steps:
carrying out uniform parametric description on the characteristics of points, lines and surfaces in the single-frame image;
converting the local characteristic element m in the current frame into a global coordinate system according to the initial pose X of the current frame in the global coordinate system to obtain a global parameter m' of the local characteristic element m; the global coordinate system is a starting frame camera coordinate system;
taking the global parameter m 'as an input node, finding a global landmark which is spatially nearest to the global parameter m' by using a KDTree search algorithm in a reference frame, a local map and a global map, and establishing a matching relation between the local feature element m of the current frame and the global landmark;
defining a uniform residual error model, calculating a matching residual error between the local feature elements of the current frame and the global landmark according to a matching relation, and iteratively solving the optimal pose of the current frame under a global coordinate system by adopting a Gauss-Newton method according to the matching residual error;
carrying out map optimization on the local map by using the optimal pose of each frame in the local map under the global coordinate system;
the unified parametric description of the point, line and surface characteristics in the single-frame image comprises the following steps:
defining three-dimensional center point p' of point p, line L and plane alpha characteristic and direction vector
3 × 3 attitude matrix R, 3 × 3 morphology matrix Ω;
wherein the three-dimensional center point p' of the point p, the line L, the plane alpha characteristic, and the direction vector
As follows:
the 3 x 3 attitude matrix R of the points p, lines L, faces α features is as follows:
the 3 x 3 morphology matrix Ω for the point p, line L, face α features is as follows:
Based on the above definition, the characteristics of the arbitrary point p, the line L, and the plane α are parameterized as: m: { p' m ,R m ,Ω m }；
The unified residual model is defined as follows:
defining the jth local feature element m of the ith frame ij :{p' ij ,R ij ,Ω ij And global signpost
7-dimensional residual model in between:
wherein the content of the first and second substances,
is the ith frame attitude matrix, SE3 is a three-dimensional special Euclidean group,
represents a 3 x 3 dimensional attitude rotation matrix of the ith frame under a global coordinate system,
representing a 3 x 1 dimensional pose translation matrix corresponding to the ith frame in the global coordinate system;
is the three-dimensional central point of the global landmark,
is the direction vector of the global signpost;
the method comprises the following steps of calculating a matching residual error between a local feature element m of a current frame and a global landmark according to a matching relation, and iteratively solving the optimal pose of the current frame under a global coordinate system by adopting a Gauss-Newton method according to the matching residual error, wherein the method comprises the following steps:
301, interpolating the pose of the current frame according to the information of the exogenous sensor or predicting the pose of the current frame based on the historical frame according to the uniform motion model
Step 302, calculating a matching residual error of each local feature element of the current frame according to the pose of the current frame according to a residual error model; defining the matching residual error of the kth local feature element as e k Calculating the matching residual error of all local feature elements of the current frame according to the following formula
Step 303, judge
Whether the iteration number is not reduced or the iteration number is maximum after the iteration is updated relative to the previous round, if so, the iteration is ended and the current iteration number is selected
As the optimal pose of the current frame, otherwise, executing step 304;
step 304, traverseThe matching residual error of each local feature element of the current frame, and the Jacobian matrix for defining the relative state quantity of the k-th residual error is J k Calculating a Jacobian matrix of the relative state quantity of each element residual according to the following formula;
step 305, respectively calculating an intermediate variable matrix: h matrix and b matrix, the calculation formula is as follows:
wherein omega k Is a residual error information matrix;
step 306, calculating a disturbance quantity delta x of pose iterative update: Δ x ← H -1 b；
Step 307, updating the predicted pose of the current frame by the disturbance amount deltax according to the following formula
And jumping to step 302;
wherein R is x ( )、R y ( )、R z () Representing the Rodrigues transformation, Δ α, rotated by a certain angle around the x, y, z axes of the local coordinate system, respectively x ，Δα y ，Δα z Is the relative rotation angle around the x, y, z axis of the local coordinate system.
2. The method according to claim 1, wherein the converting the local feature element m in the current frame into the global coordinate system according to the initial pose X of the current frame in the global coordinate system to obtain the global parameter m' of the local feature element m comprises:
acquiring a pose initial value X of a current frame under a global coordinate system; definition X ═ { R X |t X E is SE3, wherein SE3 is a three-dimensional special Euclidean group, R X Representing the corresponding 3 x 3 dimensional attitude rotation matrix, t, of the current frame under the global coordinate system X Representing a corresponding 3 x 1 dimensional pose translation matrix of the current frame under a global coordinate system;
according to the formula
And converting the local feature element m contained in the current frame into a global coordinate system to obtain a global parameter m'.
3. The method according to claim 1, wherein the step of using the global parameter m 'as an input node, finding a global landmark spatially nearest to the global parameter m' in the reference frame, the local map and the global map by using a KDTree search algorithm, and establishing a matching relationship between the local feature element m of the current frame and the global landmark comprises:
parameterizing the global landmark into space nodes according to the feature type of the global landmark and storing the space nodes as a binary tree structure;
taking the frame with the maximum matching amount in the previous tracking success or local map matching as a reference frame, and matching the local feature elements in the current frame with the global landmarks in the reference frame;
updating a local map by taking the current frame as a reference, and matching local characteristic elements in the current frame with global landmarks stored in the local map;
and if the matching of the global feature element in the current frame and the landmark in the reference frame and the matching of the local feature element in the current frame and the landmark stored in the local map both fail, performing global search and matching the current frame and the global landmark in the global map.
4. The method according to claim 3, wherein the global parameter m 'is used as an input node, a KDTree search algorithm is used to find a global landmark spatially nearest to the global parameter m' in the reference frame, the local map and the global map, and a matching relationship between the local feature element m of the current frame and the global landmark is established, further comprising: and if the feature matching of the current frame, the reference frame, the local map and the global map is not successful, adding the global parameter m' of the local feature element m into the global map as a new global landmark.
5. A device for optimizing spatial multivariate feature registration based on a unified residual error model is characterized by comprising:
the parameter description module is used for carrying out uniform parametric description on the characteristics of points, lines and surfaces in the single-frame image;
the matching module is used for converting the local characteristic element m in the current frame into a global coordinate system according to the initial pose X of the current frame in the global coordinate system to obtain a global parameter m' of the local characteristic element m; the global feature element m 'is used as an input node, a global landmark which is spatially nearest to the global parameter m' is found in a reference frame, a local map and a global map by using a KDTree search algorithm, and a matching relation between the local feature element m of the current frame and the global landmark is established;
the registration module is used for defining a uniform residual error model, calculating a matching residual error between the local feature elements of the current frame and the global landmark according to the matching relation, and iteratively solving the optimal pose of the current frame under a global coordinate system by adopting a Gauss-Newton method according to the matching residual error;
the optimization module is used for carrying out map optimization on the local map by using the optimal pose of each frame in the local map under the global coordinate system;
the unified parametric description of the point, line and surface characteristics in the single-frame image comprises the following steps:
defining three-dimensional central point p' of point p, line L and plane alpha characteristics and direction vector
3 × 3 attitude matrix R, 3 × 3 morphology matrix Ω;
wherein the three-dimensional center point p' of the point p, the line L, the plane alpha characteristic, and the direction vector
As follows:
the 3 x 3 attitude matrix R of the points p, lines L, faces α features is as follows:
the 3 x 3 morphology matrix Ω of the points p, lines L, faces α features is as follows:
Based on the above definition, the characteristics of the arbitrary point p, the line L, and the plane α are parameterized as: m: { p' m ,R m ,Ω m }；
The unified residual model is defined as follows:
defining the jth local feature element m of the ith frame ij :{p' ij ,R ij ,Ω ij And global signpost
7-dimensional residual model in between:
wherein the content of the first and second substances,
is the ith frame attitude matrix, SE3 is a three-dimensional special Euclidean group,
represents a 3 x 3 dimensional attitude rotation matrix of the ith frame under a global coordinate system,
representing a 3 x 1 dimensional pose translation matrix corresponding to the ith frame in the global coordinate system;
is the three-dimensional central point of the global landmark,
is the direction vector of the global signpost;
the method comprises the following steps of calculating a matching residual error between a local feature element m of a current frame and a global landmark according to a matching relation, and iteratively solving the optimal pose of the current frame under a global coordinate system by adopting a Gauss-Newton method according to the matching residual error, wherein the method comprises the following steps:
301, carrying out current frame pose interpolation according to exogenous sensor information or performing historical frame-based prediction according to uniform motion modelMeasuring the pose of the current frame
Step 302, calculating a matching residual error of each local feature element of the current frame according to the pose of the current frame according to a residual error model; defining the matching residual error of the kth local feature element as e k Calculating the matching residual error of all local feature elements of the current frame according to the following formula
Step 303, judge
Whether the iteration number is not reduced or the iteration number is maximum after the iteration is updated relative to the previous round, if so, the iteration is ended and the current iteration number is selected
As the optimal pose of the current frame, otherwise, executing step 304;
step 304, traversing the matching residual error of each local feature element of the current frame, and defining the Jacobian matrix of the relative state quantity of the k-th residual error as J k Calculating a Jacobian matrix of the relative state quantity of each element residual according to the following formula;
step 305, respectively calculating an intermediate variable matrix: h matrix and b matrix, the calculation formula is as follows:
wherein omega k Is a residual error information matrix;
step 306, calculating a disturbance quantity delta x of pose iterative update: Δ x ← H -1 b；
Step 307, updating the predicted pose of the current frame by the disturbance amount deltax according to the following formula
And jumping to step 302;
wherein R is x ( )、R y ( )、R z () Representing the Rodrigues transformation, Δ α, rotated by a certain angle around the x, y, z axes of the local coordinate system, respectively x ，Δα y ，Δα z Is the relative rotation angle around the x, y, z axis of the local coordinate system.
6. An electronic device, comprising:
a memory for storing a computer software program;
a processor for reading and executing the computer software program stored in the memory, thereby implementing a method for optimizing spatial multivariate feature registration based on a unified residual error model as claimed in any one of claims 1-4.
7. A non-transitory computer-readable storage medium, wherein a computer software program for implementing the unified residual model-based spatial multivariate feature registration optimization method of any one of claims 1-4 is stored in the storage medium.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010537788.5A CN111815684B (en) | 2020-06-12 | 2020-06-12 | Space multivariate feature registration optimization method and device based on unified residual error model |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010537788.5A CN111815684B (en) | 2020-06-12 | 2020-06-12 | Space multivariate feature registration optimization method and device based on unified residual error model |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111815684A CN111815684A (en) | 2020-10-23 |
| CN111815684B true CN111815684B (en) | 2022-08-02 |
Family
ID=72845050
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010537788.5A Active CN111815684B (en) | 2020-06-12 | 2020-06-12 | Space multivariate feature registration optimization method and device based on unified residual error model |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111815684B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116930864B (en) * | 2023-06-27 | 2024-02-23 | 中铁第四勘察设计院集团有限公司 | Indoor and outdoor seamless unified reference construction method and device |
| CN117853934A (en) * | 2024-03-07 | 2024-04-09 | 武汉大势智慧科技有限公司 | Visual SLAM optimization method and system based on space multivariate geometrical characteristics |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108682027A (en) * | 2018-05-11 | 2018-10-19 | 北京华捷艾米科技有限公司 | VSLAM realization method and systems based on point, line Fusion Features |
| CN109919984A (en) * | 2019-04-15 | 2019-06-21 | 武汉惟景三维科技有限公司 | A kind of point cloud autoegistration method based on local feature description's |
| CN109993113A (en) * | 2019-03-29 | 2019-07-09 | 东北大学 | A kind of position and orientation estimation method based on the fusion of RGB-D and IMU information |
| CN110930495A (en) * | 2019-11-22 | 2020-03-27 | 哈尔滨工业大学（深圳） | Multi-unmanned aerial vehicle cooperation-based ICP point cloud map fusion method, system, device and storage medium |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP3078935A1 (en) * | 2015-04-10 | 2016-10-12 | The European Atomic Energy Community (EURATOM), represented by the European Commission | Method and device for real-time mapping and localization |
| WO2017102468A1 (en) * | 2015-12-15 | 2017-06-22 | Koninklijke Philips N.V. | Image processing systems and methods |
-
2020
- 2020-06-12 CN CN202010537788.5A patent/CN111815684B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108682027A (en) * | 2018-05-11 | 2018-10-19 | 北京华捷艾米科技有限公司 | VSLAM realization method and systems based on point, line Fusion Features |
| CN109993113A (en) * | 2019-03-29 | 2019-07-09 | 东北大学 | A kind of position and orientation estimation method based on the fusion of RGB-D and IMU information |
| CN109919984A (en) * | 2019-04-15 | 2019-06-21 | 武汉惟景三维科技有限公司 | A kind of point cloud autoegistration method based on local feature description's |
| CN110930495A (en) * | 2019-11-22 | 2020-03-27 | 哈尔滨工业大学（深圳） | Multi-unmanned aerial vehicle cooperation-based ICP point cloud map fusion method, system, device and storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111815684A (en) | 2020-10-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112859859B (en) | Dynamic grid map updating method based on three-dimensional obstacle object pixel object mapping | |
| CN108242079B (en) | VSLAM method based on multi-feature visual odometer and graph optimization model | |
| CN108280866B (en) | Road point cloud data processing method and system | |
| CN112347550B (en) | Coupling type indoor three-dimensional semantic graph building and modeling method | |
| Mu et al. | Research on SLAM algorithm of mobile robot based on the fusion of 2D LiDAR and depth camera | |
| CN110181508B (en) | Three-dimensional route planning method and system for underwater robot | |
| CN111815684B (en) | Space multivariate feature registration optimization method and device based on unified residual error model | |
| CN112444246B (en) | Laser fusion positioning method in high-precision digital twin scene | |
| CN113110455A (en) | Multi-robot collaborative exploration method, device and system for unknown initial state | |
| US7030875B2 (en) | Environmental reasoning using geometric data structure | |
| CN113239072A (en) | Terminal equipment positioning method and related equipment thereof | |
| Garrote et al. | 3D point cloud downsampling for 2D indoor scene modelling in mobile robotics | |
| CN115639823A (en) | Terrain sensing and movement control method and system for robot under rugged and undulating terrain | |
| CN116698043A (en) | Visual navigation method for indoor mobile robot | |
| CN113761647B (en) | Simulation method and system of unmanned cluster system | |
| Kimmel et al. | Finding the shortest paths on surfaces by fast global approximation and precise local refinement | |
| CN115239899B (en) | Pose map generation method, high-precision map generation method and device | |
| CN113379915B (en) | Driving scene construction method based on point cloud fusion | |
| Guo et al. | Improved marching tetrahedra algorithm based on hierarchical signed distance field and multi-scale depth map fusion for 3D reconstruction | |
| Zieliński et al. | Keyframe-based dense mapping with the graph of view-dependent local maps | |
| Zhang et al. | An improvement algorithm for OctoMap based on RGB-D SLAM | |
| Li et al. | An SLAM algorithm based on laser radar and vision fusion with loop detection optimization | |
| Zhao et al. | Design of 3D reconstruction system on quadrotor Fusing LiDAR and camera | |
| Liu et al. | Laser 3D tightly coupled mapping method based on visual information | |
| CN113487485A (en) | Octree map hole completion method based on class gray level image |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |