CN117291984B - Multi-frame descriptor matching repositioning method and system based on pose constraint - Google Patents
Multi-frame descriptor matching repositioning method and system based on pose constraint Download PDFInfo
- Publication number
- CN117291984B CN117291984B CN202311560278.XA CN202311560278A CN117291984B CN 117291984 B CN117291984 B CN 117291984B CN 202311560278 A CN202311560278 A CN 202311560278A CN 117291984 B CN117291984 B CN 117291984B
- Authority
- CN
- China
- Prior art keywords
- descriptor
- pose
- matching
- frame
- probability matrix
- 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
- 238000000034 method Methods 0.000 title claims abstract description 26
- 239000011159 matrix material Substances 0.000 claims abstract description 94
- 230000007704 transition Effects 0.000 claims abstract description 86
- 238000006243 chemical reaction Methods 0.000 claims description 22
- 238000012546 transfer Methods 0.000 claims description 14
- 238000004364 calculation method Methods 0.000 claims description 5
- 238000010845 search algorithm Methods 0.000 claims description 4
- 238000012163 sequencing technique Methods 0.000 claims description 3
- 238000001514 detection method Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 235000004522 Pentaglottis sempervirens Nutrition 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
- G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
- G06V10/75—Organisation of the matching processes, e.g. simultaneous or sequential comparisons of image or video features; Coarse-fine approaches, e.g. multi-scale approaches; using context analysis; Selection of dictionaries
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
- G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
- G06V10/761—Proximity, similarity or dissimilarity measures
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20076—Probabilistic image processing
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30248—Vehicle exterior or interior
- G06T2207/30252—Vehicle exterior; Vicinity of vehicle
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Artificial Intelligence (AREA)
- Health & Medical Sciences (AREA)
- Computing Systems (AREA)
- Databases & Information Systems (AREA)
- Evolutionary Computation (AREA)
- General Health & Medical Sciences (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Multimedia (AREA)
- Image Analysis (AREA)
Abstract
The invention provides a multi-frame descriptor matching repositioning method and a multi-frame descriptor matching repositioning system based on pose constraint, wherein the method comprises the following steps: s1, acquiring a plurality of key frames and point clouds of the key frames by using a vehicle sensor, respectively establishing feature descriptors for the key frames and forming feature descriptor sets, and constructing a priori map according to the point clouds and the feature descriptor sets; s2, acquiring a current key frame, establishing feature descriptors, and respectively matching feature descriptor sets with the feature descriptors to obtain a descriptor transition state probability matrix; s3, determining the pose of the key frame according to the current adjacent key frame, and obtaining a pose transition probability matrix according to the feature descriptor of the key frame and the pose of the key frame; s4, determining posterior probability according to the descriptor transition state probability matrix and the pose transition probability matrix; s5, determining the hidden state of the current key frame according to the posterior probability, and matching the hidden state of the current key frame with the prior map to obtain a repositioning result.
Description
Technical Field
The invention relates to the technical field of automatic driving positioning, in particular to a multi-frame descriptor matching repositioning method and system based on pose constraint.
Background
In the field of autonomous and unmanned vehicles, real-time, high-precision positioning is crucial. Traditional positioning methods may not meet the requirements in complex, dynamic environments. The existing repositioning technology based on the laser radar sensor is mainly realized by matching the feature descriptors of a single real-time key frame with the prior map, however, the matching repositioning of the single frame descriptors has some problems, such as small feature information quantity, easy mismatching in similar environments, low repositioning accuracy, high initial pose estimation failure rate and lower system robustness. Some applications attempt multiple single-frame descriptor matches with a continuous number of real-time key frames during vehicle operation and select the result with the highest similarity score as the repositioning result, but are still essentially simple repetitions of single-frame descriptor matches with limited accuracy improvement.
Chinese patent CN114137560A discloses a vehicle repositioning method, device and electronic equipment based on improved laser descriptors, which utilizes a laser radar to establish a global priori 3D point cloud map in an ENU local coordinate system, constructs a laser descriptor data pair, and achieves repositioning by carrying out fine matching on coarse pose of coarse positioning and converting the local coordinate system to a geodetic coordinate system to obtain actual pose information of the vehicle.
However, the single-frame descriptor is adopted for matching and positioning, and when errors exist in the rough pose of the initial positioning, the failure of the follow-up accurate matching can be further influenced; and when the vehicle is in a complex environment, the map acquired by the laser radar is affected by environmental changes, so that the map is not completely matched with the actual environment, and the positioning accuracy is affected.
Disclosure of Invention
In view of this, the invention provides a multi-frame descriptor matching repositioning method and system based on pose constraint, which are used for calculating a descriptor state transition matrix of a current key frame relative to a feature descriptor set of a priori map by matching the feature descriptor of the current key frame with the feature descriptor set of the priori map, realizing multi-frame descriptor matching repositioning, determining posterior probability according to pose transformation transition corresponding to the key frame, matching repositioning the implicit state of the key frame corresponding to the posterior probability with the priori map, ensuring repositioning stability, and further improving repositioning accuracy.
The technical scheme of the invention is realized as follows:
in a first aspect, the present invention provides a multi-frame descriptor matching repositioning method based on pose constraint, including the following steps:
s1, acquiring a plurality of key frames and point clouds of the key frames by using a vehicle sensor, respectively establishing feature descriptors for the key frames and forming a feature descriptor set D, and constructing a priori map according to the point clouds and the feature descriptor set D;
s2, obtaining a current key frame K n c And establishes the feature descriptor D n c Respectively associating the feature descriptor set D with the feature descriptors D n c Matching to obtain a descriptor transition state probability matrix;
s3, determining the pose of the key frame according to the current adjacent key frame, and obtaining a pose transition probability matrix according to the feature descriptor of the key frame and the pose of the key frame;
s4, determining posterior probability according to the descriptor transition state probability matrix and the pose transition probability matrix;
s5, determining the hidden state of the current key frame according to the posterior probability, and matching the hidden state of the current key frame with the prior map to obtain a repositioning result.
On the basis of the above technical solution, preferably, step S2 specifically includes:
s21, searching the feature descriptor set D and the feature descriptor set D n c NS candidates with highest similarity scores, wherein NS is a natural number that is not zero;
s22, forming an implicit state set S according to the poses of the NS candidate items n Denoted as S n ={S n (i)|i=1,2，...，NS}；
S23, according to the implicit state set S n A descriptor transition state probability matrix is determined.
On the basis of the above technical solution, preferably, step S23 specifically includes:
separately computing implicit statesSet S n Corresponding feature descriptor and feature descriptor D n c Is a similarity score of (2);
respectively calculating implicit state sets S n Corresponding feature descriptor and feature descriptor D n c Is a matching distance of (2);
calculating to obtain an implicit state set S according to the similarity score and the matching distance n Corresponding feature descriptor and feature descriptor D n c Matching probabilities of (a);
and constructing a descriptor transition state probability matrix according to the matching probability.
On the basis of the above technical solution, preferably, the descriptor transition state probability matrix is:
；
wherein B is n Representing a descriptor transition state probability matrix,representing feature descriptor corresponding to mth implicit state and feature descriptor D n c Matching probability of [ (S)] T Representing the transpose of the matrix.
On the basis of the above technical solution, preferably, step S3 specifically includes:
determining the transfer distance of the pose of the current key frame according to the pose conversion between the current adjacent key frames and the pose conversion between the hidden states of the prior map adjacent key frames;
determining the matching probability of pose conversion between current adjacent key frames and pose conversion between hidden states of the prior map adjacent key frames according to the transfer distance;
and obtaining a pose transition probability matrix according to the matching probability.
On the basis of the technical scheme, preferably, the pose transition probability matrix is:
wherein,pose transition probability matrix->Representing pose conversion between current neighboring key frames +.>Pose conversion between hidden states of key frames adjacent to a priori map>Matching probabilities of (c) are determined.
On the basis of the above technical solution, preferably, step S4 specifically includes:
determining a posterior probability matrix according to the descriptor transition state probability matrix and the pose transition probability matrix;
and determining the maximum posterior probability in the posterior probability matrix according to a search algorithm.
Based on the above technical solution, preferably, the calculation formula of the posterior probability matrix is as follows:
wherein MAP is a posterior probability matrix,descriptor transition state probability matrix representing NX key frames, < >>And representing pose transition probability matrixes of NX key frames.
Still more preferably, step S5 specifically includes:
gridding the prior map, and calculating the average value and variance of each grid based on point clouds in the grids;
obtaining a probability density function of each grid according to the average value and the variance;
determining a radar frame according to a current key frame corresponding to the posterior probability, respectively calculating likelihood functions of the radar frame in a grid according to the radar frame and the probability density function, and sequencing according to the sequence from large to small;
the maximum value of the likelihood function is the relocation result.
In a second aspect, the present invention provides a multi-frame descriptor matching repositioning system based on pose constraint, which is characterized in that the multi-frame descriptor matching repositioning method described above is adopted, and the method includes:
the acquisition module is used for acquiring a plurality of key frames and point clouds of the key frames, respectively establishing characteristic descriptors for the key frames and forming a characteristic descriptor set D, and constructing a priori map according to the point clouds and the characteristic descriptor set D;
the state transition module is used for matching the feature descriptor of the current key frame with the feature descriptor set D to obtain a descriptor transition state probability matrix;
the pose transfer module is used for determining the pose of the key frame according to the current adjacent key frame and obtaining a pose transfer probability matrix according to the feature descriptor of the key frame and the pose of the key frame;
the posterior probability module is used for determining posterior probability according to the descriptor transition state probability matrix and the pose transition probability matrix;
and the matching repositioning module is used for carrying out matching repositioning according to the hidden state of the key frame corresponding to the posterior probability and the prior map to obtain a repositioning result.
Compared with the prior art, the multi-frame descriptor matching repositioning method has the following beneficial effects:
(1) Matching is carried out by setting a feature descriptor set of the prior map and a feature descriptor of the current key frame to obtain a descriptor state transition probability matrix, the pose of the key frame is utilized to determine a pose transition probability matrix, posterior probability is determined by the descriptor transition state probability matrix and the pose transition probability matrix, and the stability and accuracy of repositioning are improved by matching repositioning by utilizing the feature descriptor of the multi-frame key frame;
(2) Setting a descriptor of a current key frame and a feature descriptor set of a priori map to be matched, taking a candidate pose with highest similarity score as an implicit state set, increasing coverage range of vehicle repositioning on different positions and poses to use different positions and poses of the vehicle, and improving repositioning accuracy;
(3) The pose conversion between the current adjacent key frames and the pose conversion between the hidden states of the prior map adjacent key frames are subjected to pose conversion matching to obtain a displacement conversion probability matrix, so that the repositioning stability of the vehicle is improved, the next position of the vehicle can be predicted better under the condition of facing a complex scene, and the positioning instantaneity is improved.
Drawings
In order to more clearly illustrate the embodiments of the invention or the technical solutions in the prior art, the drawings that are required in the embodiments or the description of the prior art will be briefly described, it being obvious that the drawings in the following description are only some embodiments of the invention, and that other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a flow chart of a multi-frame descriptor matching relocation method of the present invention.
Detailed Description
The following description of the embodiments of the present invention will clearly and fully describe the technical aspects of the embodiments of the present invention, and it is apparent that the described embodiments are only some embodiments of the present invention, not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the present invention without making any inventive effort, are intended to fall within the scope of the present invention.
As shown in fig. 1, the invention provides a multi-frame descriptor matching repositioning method based on pose constraint, which comprises the following steps:
s1, acquiring a plurality of key frames and point clouds of the key frames by using a vehicle sensor, respectively establishing feature descriptors for the key frames to form a feature descriptor set D, and constructing a priori map according to the point clouds and the feature descriptor set D.
As will be appreciated by those skilled in the art, during the travel of a vehicle, point cloud data around the vehicle is acquired by an onboard sensor (e.g., camera, lidar, etc.) and a particular frame is extracted, and when the pose of the particular frame exceeds a threshold compared to the pose of the particular frame at the previous time, the particular frame is selected as a key frame, a feature descriptor is created for each key frame, and a feature descriptor set D of a priori map is formed, where d= { D n M |n=1,2，...，N}，D n M To represent the nth feature descriptor of the mth keyframe, where M is the number of keyframes and N is the number of feature descriptors in the prior map. The key frames are selected through gesture displacement, so that the number of unnecessary key frames is avoided, the complexity of map updating is reduced, the efficiency of map updating is improved, and meanwhile, scene changes can be captured more accurately, so that the accuracy of map updating is improved. The threshold may be set according to actual situations, which is not specifically limited in the present application.
Specifically, a fixed sliding window method is used for selecting a plurality of key frames, namely, a fixed number of key frames closest to the current key frame in time are selected, when new key frames are added, the key frames with the farthest time are removed, wherein the number of the key frames is N, feature descriptors are respectively built for the key frames in the sliding window, and a priori map is built according to point clouds and the feature descriptors.
In an embodiment of the present application, the feature descriptor adopts a loop detection (Scan Context) descriptor, that is, an SC descriptor, and the point cloud is divided by using a ring and a sector under a bird's eye view, so that the ring and the sector can uniquely index any region of the point cloud, and a two-dimensional feature map can be used to represent an index relationship, where a numerical value in the feature map is a highest point of the point cloud in the index region, and N is assumed s And N r Representing the number of sectors (sectors) and rings (rings), respectively, the established SC profile is N s ×N r Order descriptorMatrix I.
According to row vector r of matrix I i The feature descriptors are calculated, and the calculation function of each row vector is as follows:
wherein,computing function representing row vector,/>Representing the number of non-zero elements of the row vector;
computing function solution is carried out on each row in the descriptor matrix I to obtain a characteristic descriptor, wherein the characteristic descriptor is N r The vector k of the dimensions is used,
in the embodiment of the application, since the loop detection descriptor can provide stable feature description under different environments and illumination conditions, the loop detection descriptor is used as the feature descriptor, so that the vehicle can accurately represent the position of the point cloud even in a complex environment, the accuracy of repositioning the vehicle is improved, meanwhile, the two-dimensional feature map is used for representing the index relationship, and the position of the feature map is confirmed according to the index relationship, thereby being beneficial to improving the positioning efficiency and instantaneity.
S2, obtaining a current key frame K n c And establishes the feature descriptor D n c Respectively associating the feature descriptor set D with the feature descriptors D n c Matching is carried out, a descriptor transition state probability matrix is obtained, real-time estimation of the vehicle position is achieved, and positioning accuracy is improved.
Specifically, step S2 specifically includes:
s21, searching the feature descriptor set D and the feature descriptor set D n c The NS candidates with the highest similarity scoresWherein NS is a natural number other than zero;
s22, forming an implicit state set S according to the poses of the NS candidate items n Denoted as S n ={S n (i)|i=1,2，...，NS}；
S23, according to the implicit state set S n A descriptor transition state probability matrix is determined.
In the embodiment of the application, the feature descriptor D is searched and searched on the feature descriptor set D through the feature descriptor and the KD tree n c The NS candidates with the highest similarity scores can be set according to actual conditions, and the poses of the NS candidates form an implicit state set S n By implicit state set S n Describing the spatial relationship between the feature descriptors of the current key frame and the feature descriptors in the feature descriptor set D provides more accurate information for calculating the descriptor transition state probability matrix, thereby improving the accuracy and the robustness of vehicle state estimation.
Further, the step S23 specifically includes:
respectively calculating implicit state sets S n Corresponding feature descriptor and feature descriptor D n c Is a similarity score of (2);
respectively calculating implicit state sets S n Corresponding feature descriptor and feature descriptor D n c Is a matching distance of (2);
calculating to obtain an implicit state set S according to the similarity score and the matching distance n Corresponding feature descriptor and feature descriptor D n c Matching probabilities of (a);
and constructing a descriptor transition state probability matrix according to the matching probability, namely, the current key frame has a descriptor state transition probability matrix relative to each key frame of the prior map.
The descriptor transition state probability matrix is as follows:
wherein B is n Representing a descriptor transition state probability matrix,representing feature descriptor corresponding to mth implicit state and feature descriptor D n c Matching probability of [ (S)] T Representing the transpose of the matrix.
It can be appreciated that the similarity score is calculated as follows:
wherein I is q Representing feature descriptor D n c Feature matrix of (I) c Representing an implicit state set S n Feature matrix of corresponding feature descriptor, c j p And c j c Respectively representing feature descriptors D at the same index n c And implicit state set S n The column vector of the corresponding feature descriptor,representation c j p Vector modulus of>Representation c j c Is a vector modulus of (c).
Since rotation of the radar in the yaw direction (yaw direction) causes a characteristic translation, the formula for the matching distance is as follows:
wherein dd n (i) The minimum of (2) is the final matching distance.
The formula of the matching probability is:
wherein O is n Represent S n The corresponding feature descriptors a are adjustment parameters, and adjustment setting can be performed according to actual conditions.
In the embodiment of the application, by calculating the implicit state set S n Corresponding feature descriptor and feature descriptor D n c Similarity score and matching distance of (2), accurately evaluating implicit state set S n Corresponding feature descriptor and feature descriptor D n c Similarity and matching degree between the key frames, thereby improving matching accuracy, constructing a descriptor transition state probability matrix according to matching probability, and better describing the feature descriptor D of the current key frame n c And implicit state set S n And the feature descriptors among different corresponding feature descriptors are matched, so that the accuracy and the robustness of state estimation are improved.
S3, determining the pose of the key frame according to the current adjacent key frame, and obtaining a pose transition probability matrix according to the feature descriptors of the key frame and the pose of the key frame, so that the capability of matching, repositioning and adapting to complex environments is improved.
Specifically, step S3 specifically includes:
determining the transfer distance of the pose of the current key frame according to the pose conversion between the current adjacent key frames and the pose conversion between the hidden states of the prior map adjacent key frames;
determining the matching probability of pose conversion between current adjacent key frames and pose conversion between hidden states of the prior map adjacent key frames according to the transfer distance;
and obtaining a pose transition probability matrix according to the matching probability.
The pose transition probability matrix is as follows:
wherein,pose transition probability matrix->Representing pose conversion between current neighboring key frames +.>Pose conversion between hidden states of key frames adjacent to a priori map>Matching probabilities of (c) are determined.
Specifically, the formula of the transfer distance is:
wherein dt is n，n+1 (i, j) represents a transfer distance,representing mahalanobis distance, < >>The representation weight parameter can be set according to actual conditions.
The formula of the matching probability is:
wherein a is an adjustment parameter.
In the embodiment of the application, the pose transition distance between the current adjacent key frames and the matching probability of the pose transition between the pose transitions between the current adjacent key frames and the hidden states of the prior map adjacent key frames are calculated, and the pose transition probability matrix is constructed according to the matching probability, so that the stability of matching repositioning is improved, the matching probability between different pose transitions of the key frames can be accurately described even in a complex environment, the accuracy and the robustness of pose estimation are improved, and the instantaneity and the stability of vehicle matching repositioning are improved by constructing the pose transition probability matrix.
S4, determining posterior probability according to the descriptor transition state probability matrix and the pose transition probability matrix.
Specifically, step S4 specifically includes:
determining a posterior probability matrix according to the descriptor transition state probability matrix and the pose transition probability matrix;
and determining the maximum posterior probability in the posterior probability matrix according to a search algorithm.
The calculation formula of the posterior probability matrix is as follows:
wherein MAP is a posterior probability matrix,descriptor transition state probability matrix representing NX key frames, < >>And representing pose transition probability matrixes of NX key frames, wherein NX is a natural number which is not zero, and NX epsilon N.
In the embodiment of the application, the posterior probability matrix is calculated according to the descriptor transition state probability matrix and the pose transition probability matrix, so that the current pose state of the vehicle can be estimated more accurately, and the maximum posterior probability in the posterior probability matrix is determined according to the search algorithm, so that the pose state of the vehicle can be estimated accurately even in a complex environment, and the pose estimation accuracy is improved.
S5, determining the hidden state of the current key frame according to the posterior probability, and then matching the hidden state of the current key frame with the prior map to obtain a repositioning result, thereby improving the repositioning accuracy and instantaneity.
Specifically, step S5 specifically includes:
gridding the prior map, and calculating the average value and variance of each grid based on point clouds in the grids;
obtaining a probability density function of each grid according to the average value and the variance;
determining a radar frame according to a current key frame corresponding to the posterior probability, respectively calculating likelihood functions of the radar frame in a grid according to the radar frame and the probability density function, and sequencing according to the sequence from large to small;
the maximum value of the likelihood function is the relocation result.
In the embodiment of the application, the maximum posterior probability corresponds to the implicit state of the current key frame, the implicit state of the current key frame is matched with the prior map, the prior map is subjected to gridding processing, the probability density function of each grid is calculated, accurate positioning of the radar frame is realized in the repositioning process, the average value and the variance of each grid are calculated based on point clouds in the grids, the likelihood function of Lei Dazhen in the map grid is calculated according to the probability density function, and the repositioning accuracy and the repositioning robustness are improved.
In an embodiment of the present application, the prior map is gridded by using a grid map, that is, the point cloud of the whole space is divided by using a small cube, and for each network, the average value and the variance of the point cloud are calculated based on the points in the grid, and the formula is as follows:
wherein,represents the average value of all points in the grid, m represents the number of grids,/->Representing the position of all points within the grid, +.>Representing variance (·) T Representing the transpose of the matrix.
The probability density function can be calculated according to the average value and the variance of the grid, and the probability density function has the following formula:
where x represents the location of any point in the grid.
Determining a radar frame according to the key frame, calculating likelihood functions of all points in the grid according to the radar frame and the probability density function, and performing state estimation according to the descriptor transition state and the pose transition state of the current key frame to obtain a matching repositioning result, wherein the calculation formula of the likelihood functions is as follows:
wherein P is L K Representing the radar frame for which the current key frame is determined,representing the amount of transition of the radar frame.
The application also provides a multi-frame descriptor matching repositioning system based on pose constraint, which adopts the multi-frame descriptor matching repositioning method, and comprises the following steps:
the acquisition module is used for acquiring a plurality of key frames and point clouds of the key frames, respectively establishing characteristic descriptors for the key frames and forming a characteristic descriptor set D, and constructing a priori map according to the point clouds and the characteristic descriptor set D;
the state transition module is used for matching the feature descriptor of the current key frame with the feature descriptor set D to obtain a descriptor transition state probability matrix;
the pose transfer module is used for determining the pose of the key frame according to the current adjacent key frame and obtaining a pose transfer probability matrix according to the feature descriptor of the key frame and the pose of the key frame;
the posterior probability module is used for determining posterior probability according to the descriptor transition state probability matrix and the pose transition probability matrix;
and the matching repositioning module is used for carrying out matching repositioning according to the hidden state of the key frame corresponding to the posterior probability and the prior map to obtain a repositioning result.
According to the method, the acquisition module is used for acquiring a plurality of key frames and point clouds of the key frames, feature descriptor extraction is carried out on the point clouds of the key frames to form a feature descriptor set D, a priori map is built based on the point clouds of the key frames and the feature descriptor set D, a state transition probability matrix and a pose transition probability matrix of the current key frames are calculated by the state transition module and the posterior probability module respectively, posterior probability is calculated by the posterior probability module according to the state transition probability matrix and the vehicle pose transition probability matrix, real-time positions of vehicles are estimated by the matching repositioning module by using the posterior probability, and positioning stability and accuracy are improved.
The foregoing description of the preferred embodiments of the invention is not intended to be limiting, but rather is intended to cover all modifications, equivalents, alternatives, and improvements that fall within the spirit and scope of the invention.
Claims (8)
1. A multi-frame descriptor matching repositioning method based on pose constraint is characterized by comprising the following steps:
s1, acquiring a plurality of point cloud data and a plurality of corresponding key frames by using a vehicle sensor, respectively establishing feature descriptors for the plurality of key frames and forming a feature descriptor set D, and constructing a priori map according to the point cloud and the feature descriptor set D;
s2, obtaining a current key frame K n c And establishes the feature descriptor D n c Respectively associating the feature descriptor set D with the feature descriptors D n c Matching to obtain a descriptor transition state probability matrix;
s3, determining the pose of the current key frame according to the current adjacent key frame, and obtaining a pose transition probability matrix according to the feature descriptor of the current key frame and the pose of the current key frame;
s4, determining posterior probability according to the descriptor transition state probability matrix and the pose transition probability matrix;
s5, determining the hidden state of the current key frame according to the posterior probability, and matching the hidden state of the current key frame with the prior map to obtain a repositioning result;
the step S3 specifically comprises the following steps:
determining the transfer distance of the pose of the current key frame according to the pose conversion between the current adjacent key frames and the pose conversion between the hidden states of the prior map adjacent key frames;
determining the matching probability of pose conversion between current adjacent key frames and pose conversion between hidden states of the prior map adjacent key frames according to the transfer distance;
obtaining a pose transition probability matrix according to the matching probability;
the pose transition probability matrix is as follows:
；
wherein,pose transition probability matrix->Representing pose conversion between current neighboring key frames +.>Pose conversion between hidden states of key frames adjacent to a priori map>Matching probabilities of (c) are determined.
2. The multi-frame descriptor matching repositioning method based on pose constraint according to claim 1, wherein the step S2 specifically comprises:
s21, searching the feature descriptor set D and the feature descriptor set D n c Similarity degreeThe NS candidates with the highest scores, wherein NS is a natural number other than zero;
s22, forming an implicit state set S according to the poses of the NS candidate items n Denoted as S n ={S n (i)|i=1,2，...，NS}；
S23, according to the implicit state set S n A descriptor transition state probability matrix is determined.
3. The multi-frame descriptor matching repositioning method based on pose constraint according to claim 2, wherein the step S23 specifically comprises:
respectively calculating implicit state sets S n Corresponding feature descriptor and feature descriptor D n c Is a similarity score of (2);
respectively calculating implicit state sets S n Corresponding feature descriptor and feature descriptor D n c Is a matching distance of (2);
calculating to obtain an implicit state set S according to the similarity score and the matching distance n Corresponding feature descriptor and feature descriptor D n c Matching probabilities of (a);
and constructing a descriptor transition state probability matrix according to the matching probability.
4. The multi-frame descriptor matching repositioning method based on pose constraint of claim 3, wherein the descriptor transition state probability matrix is:
；
wherein B is n Representing a descriptor transition state probability matrix,representing feature descriptor corresponding to mth implicit state and feature descriptor D n c Matching probability of [ (S)] T Representing the transpose of the matrix.
5. The multi-frame descriptor matching repositioning method based on pose constraint according to claim 1, wherein the step S4 specifically comprises:
determining a posterior probability matrix according to the descriptor transition state probability matrix and the pose transition probability matrix;
and determining the maximum posterior probability in the posterior probability matrix according to a search algorithm.
6. The multi-frame descriptor matching repositioning method based on pose constraint according to claim 5, wherein a calculation formula of the posterior probability matrix is as follows:
；
wherein MAP is a posterior probability matrix,a descriptor transition state probability matrix representing NX key frames,and representing pose transition probability matrixes of NX key frames.
7. The multi-frame descriptor matching repositioning method based on pose constraint according to claim 1, wherein the step S5 specifically comprises:
gridding the prior map, and calculating the average value and variance of each grid based on point clouds in the grids;
obtaining a probability density function of each grid according to the average value and the variance;
determining a radar frame according to a current key frame corresponding to the posterior probability, respectively calculating likelihood functions of the radar frame in a grid according to the radar frame and the probability density function, and sequencing according to the sequence from large to small;
the maximum value of the likelihood function is the relocation result.
8. A multi-frame descriptor matching repositioning system based on pose constraint, characterized in that the multi-frame descriptor matching repositioning method according to any of claims 1-7 is adopted, comprising:
the acquisition module is used for acquiring a plurality of point cloud data and a plurality of corresponding key frames, respectively establishing characteristic descriptors for the plurality of key frames and forming a characteristic descriptor set D, and constructing a priori map according to the point cloud data and the characteristic descriptor set D;
the state transition module is used for matching the feature descriptor of the current key frame with the feature descriptor set D to obtain a descriptor transition state probability matrix;
the pose transfer module is used for determining the pose of the current key frame according to the current adjacent key frame and obtaining a pose transfer probability matrix according to the feature descriptor of the current key frame and the pose of the current key frame;
the posterior probability module is used for determining posterior probability according to the descriptor transition state probability matrix and the pose transition probability matrix;
and the matching repositioning module is used for carrying out matching repositioning according to the hidden state of the key frame corresponding to the posterior probability and the prior map to obtain a repositioning result.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311560278.XA CN117291984B (en) | 2023-11-22 | 2023-11-22 | Multi-frame descriptor matching repositioning method and system based on pose constraint |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311560278.XA CN117291984B (en) | 2023-11-22 | 2023-11-22 | Multi-frame descriptor matching repositioning method and system based on pose constraint |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117291984A CN117291984A (en) | 2023-12-26 |
| CN117291984B true CN117291984B (en) | 2024-02-09 |
Family
ID=89253809
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311560278.XA Active CN117291984B (en) | 2023-11-22 | 2023-11-22 | Multi-frame descriptor matching repositioning method and system based on pose constraint |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117291984B (en) |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112862894A (en) * | 2021-04-12 | 2021-05-28 | 中国科学技术大学 | Robot three-dimensional point cloud map construction and expansion method |
| CN113532431A (en) * | 2021-07-15 | 2021-10-22 | 贵州电网有限责任公司 | Visual inertia SLAM method for power inspection and operation |
| CN113872693A (en) * | 2021-09-29 | 2021-12-31 | 佛山市南海区广工大数控装备协同创新研究院 | Method for tightly coupling visible light positioning and depth camera based on key frame selection |
| CN113902801A (en) * | 2021-09-24 | 2022-01-07 | 四川启睿克科技有限公司 | Mobile robot repositioning method, device, equipment and storage medium |
| CN113947636A (en) * | 2021-10-19 | 2022-01-18 | 中南大学 | Laser SLAM positioning system and method based on deep learning |
| CN114137560A (en) * | 2021-11-12 | 2022-03-04 | 武汉理工大学重庆研究院 | Vehicle repositioning method and device based on improved laser descriptor and electronic equipment |
| WO2022121640A1 (en) * | 2020-12-07 | 2022-06-16 | 深圳市优必选科技股份有限公司 | Robot relocalization method and apparatus, and robot and readable storage medium |
| CN116429126A (en) * | 2023-02-22 | 2023-07-14 | 南京航空航天大学 | Quick repositioning method for ground vehicle point cloud map in dynamic environment |
| CN117053779A (en) * | 2023-07-24 | 2023-11-14 | 北京科技大学 | Tightly coupled laser SLAM method and device based on redundant key frame removal |
-
2023
- 2023-11-22 CN CN202311560278.XA patent/CN117291984B/en active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2022121640A1 (en) * | 2020-12-07 | 2022-06-16 | 深圳市优必选科技股份有限公司 | Robot relocalization method and apparatus, and robot and readable storage medium |
| CN112862894A (en) * | 2021-04-12 | 2021-05-28 | 中国科学技术大学 | Robot three-dimensional point cloud map construction and expansion method |
| CN113532431A (en) * | 2021-07-15 | 2021-10-22 | 贵州电网有限责任公司 | Visual inertia SLAM method for power inspection and operation |
| CN113902801A (en) * | 2021-09-24 | 2022-01-07 | 四川启睿克科技有限公司 | Mobile robot repositioning method, device, equipment and storage medium |
| CN113872693A (en) * | 2021-09-29 | 2021-12-31 | 佛山市南海区广工大数控装备协同创新研究院 | Method for tightly coupling visible light positioning and depth camera based on key frame selection |
| CN113947636A (en) * | 2021-10-19 | 2022-01-18 | 中南大学 | Laser SLAM positioning system and method based on deep learning |
| CN114137560A (en) * | 2021-11-12 | 2022-03-04 | 武汉理工大学重庆研究院 | Vehicle repositioning method and device based on improved laser descriptor and electronic equipment |
| CN116429126A (en) * | 2023-02-22 | 2023-07-14 | 南京航空航天大学 | Quick repositioning method for ground vehicle point cloud map in dynamic environment |
| CN117053779A (en) * | 2023-07-24 | 2023-11-14 | 北京科技大学 | Tightly coupled laser SLAM method and device based on redundant key frame removal |
Non-Patent Citations (3)
| Title |
|---|
| Keyframe-Based Visual-Inertial Online SLAM with Relocalization;Anton Kasyanov 等;arXiv：1702.02175v2;全文 * |
| 基于ORB-SLAM的室内机器人定位和三维稠密地图构建;侯荣波 等;计算机应用(第05期);全文 * |
| 基于SURF和ORB全局特征的快速闭环检测;刘国忠 等;机器人(第01期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117291984A (en) | 2023-12-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110232350B (en) | Real-time water surface multi-moving-object detection and tracking method based on online learning | |
| CN110675418B (en) | Target track optimization method based on DS evidence theory | |
| CN111797716A (en) | Single target tracking method based on Siamese network | |
| CN111781608B (en) | Moving target detection method and system based on FMCW laser radar | |
| CN108594816B (en) | Method and system for realizing positioning and composition by improving ORB-SLAM algorithm | |
| CN108428220B (en) | Automatic geometric correction method for ocean island reef area of remote sensing image of geostationary orbit satellite sequence | |
| CN108647577A (en) | A kind of pedestrian's weight identification model that adaptive difficult example is excavated, method and system | |
| CN115880364B (en) | Robot pose estimation method based on laser point cloud and visual SLAM | |
| CN105160649A (en) | Multi-target tracking method and system based on kernel function unsupervised clustering | |
| CN113327272B (en) | Robustness long-time tracking method based on correlation filtering | |
| CN109242019B (en) | Rapid detection and tracking method for optical small target on water surface | |
| CN114200477A (en) | Laser three-dimensional imaging radar ground target point cloud data processing method | |
| CN113947636B (en) | Laser SLAM positioning system and method based on deep learning | |
| CN115685185B (en) | 4D millimeter wave radar and vision fusion perception method | |
| CN112270694B (en) | Method for detecting urban environment dynamic target based on laser radar scanning pattern | |
| CN111709317A (en) | Pedestrian re-identification method based on multi-scale features under saliency model | |
| CN116758153A (en) | Multi-factor graph-based back-end optimization method for accurate pose acquisition of robot | |
| CN113989612A (en) | Remote sensing image target detection method based on attention and generation countermeasure network | |
| CN116071667A (en) | Method and system for detecting abnormal aircraft targets in specified area based on historical data | |
| CN114140700A (en) | Step-by-step heterogeneous image template matching method based on cascade network | |
| CN114332172A (en) | Improved laser point cloud registration method based on covariance matrix | |
| CN111161323B (en) | Complex scene target tracking method and system based on correlation filtering | |
| CN117291984B (en) | Multi-frame descriptor matching repositioning method and system based on pose constraint | |
| CN117053779A (en) | Tightly coupled laser SLAM method and device based on redundant key frame removal | |
| CN114413882B (en) | Global initial positioning method and device based on multi-hypothesis tracking |
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 |