CN110969649A - Matching evaluation method, medium, terminal and device of laser point cloud and map - Google Patents
Matching evaluation method, medium, terminal and device of laser point cloud and map Download PDFInfo
- Publication number
- CN110969649A CN110969649A CN201911205882.4A CN201911205882A CN110969649A CN 110969649 A CN110969649 A CN 110969649A CN 201911205882 A CN201911205882 A CN 201911205882A CN 110969649 A CN110969649 A CN 110969649A
- Authority
- CN
- China
- Prior art keywords
- point
- laser
- laser point
- closest
- matching
- 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.)
- Granted
Links
- 238000011156 evaluation Methods 0.000 title claims description 43
- 238000000034 method Methods 0.000 claims abstract description 36
- 238000003860 storage Methods 0.000 claims description 22
- 238000004590 computer program Methods 0.000 claims description 16
- 238000004364 calculation method Methods 0.000 claims description 13
- 230000004888 barrier function Effects 0.000 claims description 12
- 238000012545 processing Methods 0.000 claims description 12
- 238000010606 normalization Methods 0.000 claims description 10
- 230000000694 effects Effects 0.000 abstract description 9
- 230000006870 function Effects 0.000 description 6
- 239000000306 component Substances 0.000 description 5
- 230000008569 process Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 238000013507 mapping Methods 0.000 description 4
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 230000001174 ascending effect Effects 0.000 description 1
- 239000008358 core component Substances 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
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
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Image Analysis (AREA)
- Traffic Control Systems (AREA)
Abstract
The invention discloses a method, a medium, a terminal and a device for evaluating matching of laser point cloud and a map, wherein the method comprises the following steps: rasterizing the laser point cloud on an environment map according to the prior pose of the laser point cloud at the current moment; calculating a closest point set corresponding to each laser point on the environment map; calculating the mean value points of all the closest points in the closest point set, and generating the matching score of the laser point according to the distance between the laser point and the corresponding mean value point; and summing the matching scores of all the laser points to generate a matching total score of the laser point cloud and the environment map. The method has stronger adaptability to points with larger errors, can more accurately reflect the actual effect of matching the point cloud and the map, is not easy to fall into a local minimum value, improves the matching precision and fault tolerance, can more quickly converge to an optimal solution under the condition of ensuring the operation speed, and has important significance in practical application.
Description
[ technical field ] A method for producing a semiconductor device
The invention relates to the field of navigation and positioning, in particular to a method, a medium, a terminal and a device for evaluating matching of laser point cloud and a map.
[ background of the invention ]
Laser slam (simultaneous localization and mapping) is the core technology of mobile robot positioning and navigation, and is called simultaneous positioning and mapping. When the robot is located at an unknown position in an unknown environment, the real-time pose of the robot can be estimated while the robot moves on the basis of the sensor carried by the robot through a laser slam technology, and then the map is continuously expanded and updated to gradually build a complete map of the environment. At present, two main flow directions of the laser slam are particle filtering and graph optimization methods, both of which involve matching of point cloud and grid map and are core components of the laser slam. The matching precision has a large influence on the positioning mapping precision, and meanwhile, the matching speed influences the real-time performance of the positioning mapping, and further influences the response speed and the smoothness of the robot navigation control. Therefore, the improvement of the matching precision and speed of the point cloud and the map is a key technology for the high-precision and high-efficiency operation of the robot. And when an environment map which is being constructed is known, rasterizing the point cloud according to the prior pose of the point cloud origin on the map, calculating to obtain the grid of the map where each point of the point cloud is located, and matching the rasterized point cloud with the map to obtain the posterior pose of the point cloud origin on the map, so that the accurate posterior pose of the robot carrying the laser in the map can be determined. Ideally, the grids where the point clouds are located are all in an occupied state, i.e., represent objects or obstacle points in the environment. And the matching evaluation standard of the point cloud and the map adopts a residual error or a mode of converting the residual error into a score. Determining the closest point of the point cloud in the grid map according to some searching methods, calculating the distance between the point cloud and the closest point to be called residual error, or calculating the score of positive correlation according to the distance, wherein the smaller the total residual error of the point cloud matching or the higher the score is, the better the matching effect is. The good matching effect evaluation method can reflect the actual matching effect, is less influenced by the points with larger errors, and has strong fault tolerance. The point cloud and map matching is converted along the direction with smaller residual error or higher score, so that the better matching evaluation method not only obtains better point cloud map matching precision, but also converges to the optimal value more quickly, and avoids falling into a local minimum value. Currently, an evaluation method of point-to-point distance is mainly adopted, the method uses the laser point cloud, the information of the nearest points around the laser point cloud is less, and the error of some nearest points on a map is larger, so that the evaluation of the points is deviated, and the information of the actual environment cannot be reflected. For example, some points are obstacle points generated by a moving object on a map, and then along with the movement of the object, the current obstacle points are used for matching evaluation, so that evaluation indexes are not accurate enough, and the actual effect of matching point cloud and the map cannot be reflected accurately. The method for matching the point cloud with the map moves along the direction with higher evaluation score or smaller residual error, and estimates the posterior pose of the laser point cloud on the map, if the matched evaluation index is not accurate, lower matching precision is easy to cause, the optimal solution of matching can be missed, and even the optimal solution falls into a local minimum value. Other methods adopt bi-quadratic linear interpolation, select a plurality of points close to the point cloud and corresponding probability values thereof, and calculate the probability value of the grid where the laser point is located. The discreteness of the occupied grid map limits the accuracy which can be realized by the method, and does not allow interpolation values or derivatives to be directly calculated, so that only approximate calculation is performed, larger calculation amount is caused, and meanwhile, the approximate result is influenced by the resolution of the grid map.
[ summary of the invention ]
The invention provides a method, a medium, a terminal and a device for evaluating matching of laser point cloud and a map, which solve the technical problems.
The technical scheme for solving the technical problems is as follows: a matching evaluation method of laser point cloud and a map comprises the following steps:
step 1, rasterizing laser point cloud on an environment map according to the prior pose of the laser point cloud at the current moment;
step 2, calculating a closest point set corresponding to each laser point in the laser point cloud on the environment map;
step 3, calculating the mean value point of all the closest points in the closest point set, and generating the matching score of the laser point according to the distance between the laser point and the corresponding mean value point;
and 4, summing the matching scores of all laser points in the laser point cloud to generate a total matching score of the laser point cloud and the environment map.
In a preferred embodiment, the step of calculating the closest point set corresponding to each laser point in the laser point cloud on the environment map includes the following steps:
s201, acquiring a nearest point circle of each laser point in the laser point cloud on the environment map, wherein the circle center of the nearest point circle is the center of the grid lattice where the laser point is located, and the radius is a preset value;
s202, all barrier points of each laser point in a preset search range are obtained, the barrier points in the corresponding nearest point circle range are used as nearest points, and world coordinates of the nearest points are stored to form a nearest point set corresponding to the laser points.
In a preferred embodiment, the following steps are further included between step 3 and step 4: when the closest point set does not have a closest point, taking a closest obstacle point which is out of the circle range of the closest point and is closest to the laser point in the obstacle points as a target obstacle point, calculating a first Euler distance between the laser point and the corresponding target obstacle point, and calculating a matching score of the laser point according to a first preset formula, wherein the first preset formula is as follows:
wherein d isiThe first Euler distance between the laser point i and the target obstacle point, and r is the preset radius of the nearest point circle;
when the target obstacle point is 0, the matching score of the laser spot is 0.
In a preferred embodiment, in step 3, when the closest point set includes a closest point, the closest point is an average value point; when the closest point set comprises more than two closest points, calculating the mean value point of all the closest points according to the weight of each closest point in the closest point set, and specifically comprising the following steps:
calculating a second Euler distance between the laser point and each nearest point, and taking the reciprocal of the second Euler distance as the weight of each nearest point;
and carrying out normalization processing on all weights, and solving a weighted average value of the obstacle point world coordinates corresponding to all the nearest points respectively by adopting the weights after the normalization processing, wherein the weighted average value is the average value point coordinates of all the nearest points.
In a preferred embodiment, in the step 3, generating a matching score of each laser point in the laser point cloud according to a distance between the laser point and the corresponding average point specifically includes:
calculating a third Euler distance between the laser point and the corresponding average value point, and substituting the third Euler distance into a second preset formula to generate a matching score of the laser point, wherein the second preset formula is as follows:
A second aspect of the embodiments of the present invention provides a computer-readable storage medium, which stores a computer program, and when the computer program is executed by a processor, the method for evaluating matching between a laser point cloud and a map is implemented.
The third aspect of the embodiment of the present invention provides a matching evaluation terminal for a laser point cloud and a map, which includes the computer-readable storage medium and a processor, wherein the processor implements the steps of the above matching evaluation method for a laser point cloud and a map when executing a computer program on the computer-readable storage medium.
The fourth aspect of the embodiment of the invention provides a matching evaluation device of laser point cloud and map, which comprises a rasterization module, a nearest point set acquisition module, a calculation module and a summation module,
the rasterization module is used for rasterizing the laser point cloud on an environment map according to the prior pose of the laser point cloud at the current moment;
the nearest point set acquisition module is used for calculating a nearest point set corresponding to each laser point in the laser point cloud on the environment map;
the calculation module is used for calculating the mean value point of all the closest points in the closest point set and generating the matching score of the laser point according to the distance between the laser point and the corresponding mean value point;
the summation module is used for summing the matching scores of all laser points in the laser point cloud to generate a total matching score of the laser point cloud and the environment map.
In a preferred embodiment, the closest point set obtaining module specifically includes:
a nearest point circle acquiring unit, configured to acquire a nearest point circle of each laser point in the laser point cloud on the environment map, where a circle center of the nearest point circle is a center of the grid lattice where the laser point is located, and a radius of the nearest point circle is a preset value;
and the closest point set acquisition unit is used for acquiring all barrier points of each laser point in a preset search range, taking the barrier points in the corresponding closest point circle range as the closest points, and storing world coordinates of the closest points to form a closest point set corresponding to the laser points.
In a preferred embodiment, the method further includes a second calculating module, where the second calculating module is specifically configured to, when there is no closest point in the set of closest points, take an obstacle point that is out of a circle of the closest point and closest to the laser point in the obstacle points as a target obstacle point, calculate a first euler distance between the laser point and the corresponding target obstacle point, and calculate a matching score of the laser point according to a first preset formula, where the first preset formula is:
wherein d isiThe first Euler distance between the laser point i and the target obstacle point, and r is the preset radius of the nearest point circle; when the target obstacle point is 0, the matching score of the laser point is calculated to be 0.
The invention provides a method, a medium, a terminal and a device for evaluating matching of laser point cloud and a map, which are characterized by firstly calculating all nearest points in a certain range around a grid where each point of the laser point cloud is located, obtaining the weight of each point according to the Euler distance from each nearest point to the point corresponding to the point cloud, then carrying out normalization processing on the weight, weighting the coordinates of obstacle points of a nearest point set to obtain an average point, representing the whole nearest point set by the average point, then calculating the Euler distance from the average point to the point corresponding to the point cloud, and taking the distance as the standard of point cloud evaluation. Therefore, each nearest point has a contribution value to the matching score of the point cloud, the influence on the point cloud matching score is larger if the weight is larger, the calculation of the matching score does not depend on a single point in the map completely, but depends on a mean value point obtained by weighting the nearest point set in a certain range, and therefore, the method has stronger adaptability to some points with larger errors, is more accurate in matching evaluation, can reflect the actual matching effect of the point cloud and the map more accurately, is not easy to fall into a local minimum value, better accords with the actual matching condition, improves the matching precision and fault tolerance, can quickly converge to the optimal solution under the condition of ensuring the operation speed, and has important significance in practical application.
In order to make the aforementioned and other objects, features and advantages of the present invention comprehensible, preferred embodiments accompanied with figures are described in detail below.
[ description of the drawings ]
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings needed to be used in the embodiments will be briefly described below, it should be understood that the following drawings only illustrate some embodiments of the present invention and therefore should not be considered as limiting the scope, and for those skilled in the art, other related drawings can be obtained according to the drawings without inventive efforts.
Fig. 1 is a schematic flowchart of a method for evaluating matching between a laser point cloud and a map provided in embodiment 1;
fig. 2 is a schematic structural diagram of a matching evaluation device of a laser point cloud and a map provided in embodiment 2;
fig. 3 is a schematic structural diagram of a matching evaluation terminal of a laser point cloud and a map provided in embodiment 3.
[ detailed description ] embodiments
In order to make the objects, technical solutions and advantageous effects of the present invention more clearly apparent, the present invention is further described in detail below with reference to the accompanying drawings and the detailed description. It should be understood that the detailed description and specific examples, while indicating the preferred embodiment of the invention, are intended for purposes of illustration only and are not intended to limit the scope of the invention.
Fig. 1 is a schematic flow chart of a method for evaluating matching between a laser point cloud and a map according to embodiment 1 of the present invention, as shown in fig. 1, including the following steps:
step 1, rasterizing laser point cloud on an environment map according to the prior pose of the laser point cloud at the current moment;
step 2, calculating a closest point set corresponding to each laser point in the laser point cloud on the environment map;
step 3, calculating the mean value point of all the closest points in the closest point set, and generating the matching score of the laser point according to the distance between the laser point and the corresponding mean value point;
and 4, summing the matching scores of all laser points in the laser point cloud to generate a total matching score of the laser point cloud and the environment map.
The steps of the above examples are explained in detail below.
When an environment map M for matching is known, the prior pose at the time t is obtained by the laser point cloud for matching according to the posterior pose at the time t-1 and the variable quantity from the time t-1 to the time t, and then the posterior pose at the time t can be obtained by matching the point cloud with the environment map, namely the corrected pose is obtained by accurate matching. Firstly, rasterizing the laser point cloud on an environment map based on the prior pose at the time t to obtain a grid where each point of the laser point cloud is located, and then searching for the grid marked as occupied in the range of a circle with the radius r around each point of the laser point cloud. In this embodiment, the preset r size is 1 to 2 grid resolution sizes, preferably 1.5 grid resolution sizes, and the circle center is the center of the grid where each laser point of the laser point cloud is located, so that the formed circle is the nearest point circle of each point of the laser point cloud.
Let any laser point in the laser point cloud be piAcquiring all barrier points of each laser point in a preset search range, namely searching the laser point piGrid m is locatediThe state of all the grids in the preset search range around can be searched, in the preferred embodiment, the state of 6-10 grids around can be searched, in order to ensure the matching precision and the matching speed, the state of 8 grids around is preferably searched, and when the grids are in an occupied state, the grids are obstacle points, so that all the obstacle points of each laser point in the preset search range are obtainedThen remove the laser spot p not existing on the laser spotiAnd corresponding to the barrier points in the nearest point circle range, taking the barrier points in the nearest point circle range as nearest points, and indexing and storing the world coordinates of the barrier points to form a nearest point set corresponding to the laser points. To improve the matching efficiency, p of the laser spot may be used in a preferred embodimentiNearest point set according to point piThe euler distances of the coordinates are arranged in ascending order, thereby facilitating the weighting calculation in the subsequent past.
And then, calculating the weight of each closest point in the closest point set according to the following method, and then normalizing the weight, wherein the weight reflects the distance of each closest point close to the corresponding laser point in the point cloud, and the point with the larger weight has larger influence on the result of the evaluation score. In order to improve the calculation efficiency of the matching score on the premise of ensuring good matching evaluation, in the preferred embodiment, the weight of each closest point in the closest point set and the obstacle point coordinates of each closest point are weighted and calculated to obtain the mean value of all the closest points, then the euler distances of the mean value and the stress light points in the laser point cloud are calculated, the matching score of each laser point is calculated according to the euler distances, and then the matching scores of each laser point in the point cloud are summed.
Obtaining different matching effect score calculation methods according to different numbers of points contained in the nearest point set:
(1) when there is no closest point in the closest point set, the closest point outside the circle of the closest point and closest to the laser point in the obstacle points is used as a target obstacle point, for example, find an obstacle point q outside the circle of the closest point, within 8 grids around the laser point and closest to the stress light pointiCalculating piAnd q isiFirst euler distance d ofiTo obtain piThe matching score of the points is
Where r is the preset radius of the nearest point circle. If no target obstacle point is found, i.e. no obstacle point is found within the preset search range of the laser point, then piThe point score is 0.
(2) When the closest point set comprises 1 closest point qiThen the nearest point is the average value point hiCalculating the laser point piAnd the mean point hiThird euler distance fiTo obtain piThe matching score of the points is
(3) When the closest point set comprises 2 closest points, setting the closest point as a closest point qi1Nearest point qi2Calculating the closest point qi1Nearest point qi2Respectively with point piRespectively d as a second Euler distancei1，di2The weights of the two closest points can be calculated as
The weights of the closest points obtained by weight normalization are respectively
Thus weighting to obtain the mean h of the two closest pointsiCoordinate is hi＝ωi1qi1+ωi2qi2Calculating the laser point piAnd the mean point hiThird euler distance fiTo obtain piThe matching score of the points is
(4) When the closest point set comprises n closest points, the number is { qi1,...,qinAll the nearest points and the point p are calculatediIs recorded as dij＝{di1,...dinJ e (1, n), so as to obtain the weight of each nearest point as
The weights of the closest points obtained by weight normalization are respectively
Thus weighting to obtain the mean point h of the n closest pointsiThe coordinates are
Calculating the point piAnd the mean point hiThird euler distance fiTo obtain piThe matching score of the points is
And finally, summing the matching scores of all laser points in the laser point cloud to obtain a total score of one-time matching iteration of the laser point cloud and the environment map.
The preferred embodiment provides a method, a medium, a terminal and a device for evaluating matching of a laser point cloud and a map, and the method comprises the steps of firstly calculating all nearest points in a certain range around a grid where each point of the laser point cloud is located, obtaining the weight of each point according to the Euler distance from each nearest point to the point corresponding to the point cloud, then carrying out weight normalization processing on the weights, weighting the coordinates of obstacle points of a nearest point set to obtain an average point, representing the whole nearest point set by the average point, then calculating the Euler distance from the average point to the point corresponding to the point cloud, and taking the distance as the standard of point cloud evaluation. Therefore, each nearest point has a contribution value to the matching score of the point cloud, the influence on the point cloud matching score is larger if the weight is larger, the calculation of the matching score does not depend on a single point in the map completely, but depends on a mean value point obtained by weighting the nearest point set in a certain range, and therefore, the method has stronger adaptability to some points with larger errors, is more accurate in matching evaluation, can reflect the actual matching effect of the point cloud and the map more accurately, is not easy to fall into a local minimum value, better accords with the actual matching condition, improves the matching precision and fault tolerance, can quickly converge to the optimal solution under the condition of ensuring the operation speed, and has important significance in practical application.
It should be understood that, the sequence numbers of the steps in the foregoing embodiments do not imply an execution sequence, and the execution sequence of each process should be determined by its function and inherent logic, and should not constitute any limitation to the implementation process of the embodiments of the present invention.
The embodiment of the invention also provides a computer readable storage medium, which stores a computer program, and when the computer program is executed by a processor, the method for evaluating the matching of the laser point cloud and the map is realized.
Fig. 2 is a schematic structural diagram of a matching evaluation apparatus for a laser point cloud and a map according to embodiment 2 of the present invention, as shown in fig. 2, including a rasterizing module 100, a closest point set obtaining module 200, a calculating module 300 and a summing module 400,
the rasterizing module 100 is configured to rasterize the laser point cloud on an environment map according to a prior pose of the laser point cloud at a current time;
the closest point set obtaining module 200 is configured to calculate a closest point set corresponding to each laser point in the laser point cloud on the environment map;
the calculation module 300 is configured to calculate a mean point of all closest points in the closest point set, and generate a matching score of the laser point according to a distance between the laser point and a corresponding mean point;
the summation module 400 is configured to sum the matching scores of all the laser points in the laser point cloud, and generate a total matching score between the laser point cloud and the environment map.
In a preferred embodiment, the closest point set obtaining module 200 specifically includes:
a nearest point circle acquiring unit 201, configured to acquire a nearest point circle of each laser point in the laser point cloud on the environment map, where a circle center of the nearest point circle is a center of the grid lattice where the laser point is located, and a radius of the nearest point circle is a preset value;
a closest point set obtaining unit 202, configured to obtain all obstacle points of each laser point within a preset search range, use an obstacle point within a corresponding closest point circle range as a closest point, and store world coordinates of the closest point to form a closest point set corresponding to the laser point.
In another preferred embodiment, the second calculating module 500 is further included, and the second calculating module 500 is specifically configured to, when there is no closest point in the closest point set, take an obstacle point, which is out of a circle of the closest point and closest to the laser point, of the obstacle points as a target obstacle point, calculate a first euler distance between the laser point and the corresponding target obstacle point, and calculate a matching score of the laser point according to a first preset formula, where the first preset formula is:
wherein d isiThe first Euler distance between the laser point i and the target obstacle point, and r is the preset radius of the nearest point circle; when the target obstacle point is 0, the matching score of the laser point is calculated to be 0.
In another preferred embodiment, the computing module 300 comprises:
an average point coordinate calculation unit 301, configured to, when the closest point set includes one closest point, take the closest point as an average point; when the closest point set comprises more than two closest points, calculating a second Euler distance between the laser point and each closest point, taking the reciprocal of the second Euler distance as the weight of each closest point, carrying out normalization processing on all weights, and calculating the weighted average value of the world coordinates of the obstacle points corresponding to all closest points respectively by adopting the weights after the normalization processing, wherein the weighted average value is the average point coordinate of all closest points;
a matching score calculating unit, configured to calculate a third euler distance between the laser point and the corresponding average point, and bring the third euler distance into a second preset formula to generate a matching score of the laser point, where the second preset formula is:
The embodiment of the invention also provides a matching evaluation terminal of the laser point cloud and the map, which comprises the computer readable storage medium and a processor, wherein the processor realizes the steps of the matching evaluation method of the laser point cloud and the map when executing the computer program on the computer readable storage medium. Fig. 3 is a schematic structural diagram of a matching evaluation terminal for laser point cloud and map provided in embodiment 3 of the present invention, and as shown in fig. 3, the matching evaluation terminal 8 for laser point cloud and map of this embodiment includes: a processor 80, a readable storage medium 81 and a computer program 82 stored in said readable storage medium 81 and executable on said processor 80. The processor 80, when executing the computer program 82, implements the steps in the various method embodiments described above, such as steps 1 through 4 shown in fig. 1. Alternatively, the processor 80, when executing the computer program 82, implements the functions of the modules in the above-described device embodiments, such as the functions of the modules 100 to 400 shown in fig. 2.
Illustratively, the computer program 82 may be partitioned into one or more modules that are stored in the readable storage medium 81 and executed by the processor 80 to implement the present invention. The one or more modules may be a series of computer program instruction segments capable of performing specific functions, which are used for describing the execution process of the computer program 82 in the matching evaluation terminal 8 of the laser point cloud and the map.
The matching evaluation terminal 8 of the laser point cloud and the map can include, but is not limited to, a processor 80 and a readable storage medium 81. Those skilled in the art will understand that fig. 3 is only an example of the matching evaluation terminal 8 of the laser point cloud and the map, and does not constitute a limitation on the matching evaluation terminal 8 of the laser point cloud and the map, and may include more or less components than those shown in the drawings, or combine some components, or different components, for example, the matching evaluation terminal of the laser point cloud and the map may further include a power management module, an arithmetic processing module, an input-output device, a network access device, a bus, and the like.
The Processor 80 may be a Central Processing Unit (CPU), other general purpose Processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), a Field-Programmable Gate Array (FPGA) or other Programmable logic device, discrete Gate or transistor logic device, discrete hardware component, or the like. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.
The readable storage medium 81 may be an internal storage unit of the matching evaluation terminal 8 for the laser point cloud and the map, for example, a hard disk or a memory of the matching evaluation terminal 8 for the laser point cloud and the map. The readable storage medium 81 may also be an external storage device of the matching evaluation terminal 8 for the laser point cloud and the map, for example, a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) Card, a Flash memory Card (Flash Card), or the like, which is equipped on the matching evaluation terminal 8 for the laser point cloud and the map. Further, the readable storage medium 81 may also include both an internal storage unit and an external storage device of the matching evaluation terminal 8 of the laser point cloud and the map. The readable storage medium 81 is used for storing the computer program and other programs and data required by the matching evaluation terminal of the laser point cloud and the map. The readable storage medium 81 may also be used to temporarily store data that has been output or is to be output.
It will be apparent to those skilled in the art that, for convenience and brevity of description, only the above-mentioned division of the functional units and modules is illustrated, and in practical applications, the above-mentioned function distribution may be performed by different functional units and modules according to needs, that is, the internal structure of the apparatus is divided into different functional units or modules to perform all or part of the above-mentioned functions. Each functional unit and module in the embodiments may be integrated in one processing unit, or each unit may exist alone physically, or two or more units are integrated in one unit, and the integrated unit may be implemented in a form of hardware, or in a form of software functional unit. In addition, specific names of the functional units and modules are only for convenience of distinguishing from each other, and are not used for limiting the protection scope of the present application. The specific working processes of the units and modules in the system may refer to the corresponding processes in the foregoing method embodiments, and are not described herein again.
In the above embodiments, the descriptions of the respective embodiments have respective emphasis, and reference may be made to the related descriptions of other embodiments for parts that are not described or illustrated in a certain embodiment.
Those of ordinary skill in the art will appreciate that the various illustrative elements and method steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware, or combinations of computer software and electronic hardware. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the implementation. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present invention.
In the embodiments provided in the present invention, it should be understood that the disclosed apparatus/terminal device and method may be implemented in other ways. For example, the above-described embodiments of the apparatus/terminal device are merely illustrative, and for example, the division of the modules or units is only one logical division, and there may be other divisions when actually implemented, for example, a plurality of units or components may be combined or integrated into another system, or some features may be omitted, or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection through some interfaces, devices or units, and may be in an electrical, mechanical or other form.
The units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiment.
In addition, functional units in the embodiments of the present invention may be integrated into one processing unit, or each unit may exist alone physically, or two or more units are integrated into one unit. The integrated unit can be realized in a form of hardware, and can also be realized in a form of a software functional unit.
The invention is not limited solely to that described in the specification and embodiments, and additional advantages and modifications will readily occur to those skilled in the art, so that the invention is not limited to the specific details, representative apparatus, and illustrative examples shown and described herein, without departing from the spirit and scope of the general concept as defined by the appended claims and their equivalents.
Claims (10)
1. A matching evaluation method of laser point cloud and a map is characterized by comprising the following steps:
step 1, rasterizing laser point cloud on an environment map according to the prior pose of the laser point cloud at the current moment;
step 2, calculating a closest point set corresponding to each laser point in the laser point cloud on the environment map;
step 3, calculating the mean value point of all the closest points in the closest point set, and generating the matching score of the laser point according to the distance between the laser point and the corresponding mean value point;
and 4, summing the matching scores of all laser points in the laser point cloud to generate a total matching score of the laser point cloud and the environment map.
2. The method for evaluating matching between a laser point cloud and a map according to claim 1, wherein calculating the closest point set corresponding to each laser point in the laser point cloud on the environment map comprises the following steps:
s201, acquiring a nearest point circle of each laser point in the laser point cloud on the environment map, wherein the circle center of the nearest point circle is the center of the grid lattice where the laser point is located, and the radius is a preset value;
s202, all barrier points of each laser point in a preset search range are obtained, the barrier points in the corresponding nearest point circle range are used as nearest points, and world coordinates of the nearest points are stored to form a nearest point set corresponding to the laser points.
3. The method for evaluating the matching of the laser point cloud and the map according to claim 2, further comprising the following steps between the step 3 and the step 4: when the closest point set does not have a closest point, taking a closest obstacle point which is out of the circle range of the closest point and is closest to the laser point in the obstacle points as a target obstacle point, calculating a first Euler distance between the laser point and the corresponding target obstacle point, and calculating a matching score of the laser point according to a first preset formula, wherein the first preset formula is as follows:
wherein d isiThe first Euler distance between the laser point i and the target obstacle point, and r is the preset radius of the nearest point circle;
when the target obstacle point is 0, the matching score of the laser spot is 0.
4. The method for evaluating the matching between the laser point cloud and the map according to any one of claims 1 to 3, wherein in the step 3, when the closest point set comprises a closest point, the closest point is an average value point; when the closest point set comprises more than two closest points, calculating the mean value point of all the closest points according to the weight of each closest point in the closest point set, and specifically comprising the following steps:
calculating a second Euler distance between the laser point and each nearest point, and taking the reciprocal of the second Euler distance as the weight of each nearest point;
and carrying out normalization processing on all weights, and solving a weighted average value of the obstacle point world coordinates corresponding to all the nearest points respectively by adopting the weights after the normalization processing, wherein the weighted average value is the average value point coordinates of all the nearest points.
5. The method for evaluating matching between a laser point cloud and a map according to claim 4, wherein in the step 3, the step of generating the matching score of each laser point in the laser point cloud according to the distance between the laser point and the corresponding mean point specifically comprises the following steps:
calculating a third Euler distance between the laser point and the corresponding average value point, and substituting the third Euler distance into a second preset formula to generate a matching score of the laser point, wherein the second preset formula is as follows:
6. A computer-readable storage medium, in which a computer program is stored, and when the computer program is executed by a processor, the method for evaluating matching of a laser point cloud and a map according to any one of claims 1 to 5 is implemented.
7. A matching evaluation terminal of a laser point cloud and a map, comprising the computer-readable storage medium of claim 6 and a processor, wherein the processor implements the steps of the matching evaluation method of the laser point cloud and the map according to any one of claims 1 to 5 when executing the computer program on the computer-readable storage medium.
8. A matching evaluation device of laser point cloud and map is characterized by comprising a rasterization module, a nearest point set acquisition module, a calculation module and a summation module,
the rasterization module is used for rasterizing the laser point cloud on an environment map according to the prior pose of the laser point cloud at the current moment;
the nearest point set acquisition module is used for calculating a nearest point set corresponding to each laser point in the laser point cloud on the environment map;
the calculation module is used for calculating the mean value point of all the closest points in the closest point set and generating the matching score of the laser point according to the distance between the laser point and the corresponding mean value point;
the summation module is used for summing the matching scores of all laser points in the laser point cloud to generate a total matching score of the laser point cloud and the environment map.
9. The device for evaluating matching of a laser point cloud and a map according to claim 8, wherein the closest point set obtaining module specifically comprises:
a nearest point circle acquiring unit, configured to acquire a nearest point circle of each laser point in the laser point cloud on the environment map, where a circle center of the nearest point circle is a center of the grid lattice where the laser point is located, and a radius of the nearest point circle is a preset value;
and the closest point set acquisition unit is used for acquiring all barrier points of each laser point in a preset search range, taking the barrier points in the corresponding closest point circle range as the closest points, and storing world coordinates of the closest points to form a closest point set corresponding to the laser points.
10. The apparatus for evaluating matching between a laser point cloud and a map according to claim 9, further comprising a second calculating module, wherein the second calculating module is specifically configured to, when there is no closest point in the set of closest points, take an obstacle point of the obstacle points that is outside a circle of the closest point and closest to the laser point as a target obstacle point, calculate a first euler distance between the laser point and the corresponding target obstacle point, and calculate a matching score of the laser point according to a first preset formula, where the first preset formula is:
wherein d isiThe first Euler distance between the laser point i and the target obstacle point, and r is the preset radius of the nearest point circle; when the target obstacle point is 0, the matching score of the laser point is calculated to be 0.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911205882.4A CN110969649B (en) | 2019-11-29 | 2019-11-29 | Matching evaluation method, medium, terminal and device for laser point cloud and map |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911205882.4A CN110969649B (en) | 2019-11-29 | 2019-11-29 | Matching evaluation method, medium, terminal and device for laser point cloud and map |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110969649A true CN110969649A (en) | 2020-04-07 |
| CN110969649B CN110969649B (en) | 2024-04-05 |
Family
ID=70032557
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911205882.4A Active CN110969649B (en) | 2019-11-29 | 2019-11-29 | Matching evaluation method, medium, terminal and device for laser point cloud and map |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110969649B (en) |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111707279A (en) * | 2020-05-19 | 2020-09-25 | 上海有个机器人有限公司 | Matching evaluation method, medium, terminal and device of laser point cloud and map |
| CN111707262A (en) * | 2020-05-19 | 2020-09-25 | 上海有个机器人有限公司 | Point cloud matching method, medium, terminal and device based on closest point vector projection |
| CN111765882A (en) * | 2020-06-18 | 2020-10-13 | 浙江大华技术股份有限公司 | Laser radar positioning method and related device thereof |
| CN111812613A (en) * | 2020-08-06 | 2020-10-23 | 常州市贝叶斯智能科技有限公司 | Mobile robot positioning monitoring method, device, equipment and medium |
| CN111895989A (en) * | 2020-06-24 | 2020-11-06 | 浙江大华技术股份有限公司 | Robot positioning method and device and electronic equipment |
| CN112258618A (en) * | 2020-11-04 | 2021-01-22 | 中国科学院空天信息创新研究院 | Semantic mapping and positioning method based on fusion of prior laser point cloud and depth map |
| CN112462385A (en) * | 2020-10-21 | 2021-03-09 | 南开大学 | Map splicing and positioning method based on laser radar under outdoor large environment |
| CN112634260A (en) * | 2020-12-31 | 2021-04-09 | 上海商汤智能科技有限公司 | Map evaluation method and device, electronic equipment and storage medium |
| CN112710312A (en) * | 2020-12-24 | 2021-04-27 | 哈尔滨工业大学芜湖机器人产业技术研究院 | Laser SLAM front-end matching method fusing distance residual and probability residual |
| CN113029137A (en) * | 2021-04-01 | 2021-06-25 | 清华大学 | Multi-source information self-adaptive fusion positioning method and system |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107144292A (en) * | 2017-06-08 | 2017-09-08 | 杭州南江机器人股份有限公司 | The odometer method and mileage counter device of a kind of sports equipment |
| CN108732584A (en) * | 2017-04-17 | 2018-11-02 | 百度在线网络技术（北京）有限公司 | Method and apparatus for updating map |
| US20190146062A1 (en) * | 2017-11-15 | 2019-05-16 | Baidu Online Network Technology (Beijing) Co., Ltd | Laser point cloud positioning method and system |
-
2019
- 2019-11-29 CN CN201911205882.4A patent/CN110969649B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108732584A (en) * | 2017-04-17 | 2018-11-02 | 百度在线网络技术（北京）有限公司 | Method and apparatus for updating map |
| CN107144292A (en) * | 2017-06-08 | 2017-09-08 | 杭州南江机器人股份有限公司 | The odometer method and mileage counter device of a kind of sports equipment |
| US20190146062A1 (en) * | 2017-11-15 | 2019-05-16 | Baidu Online Network Technology (Beijing) Co., Ltd | Laser point cloud positioning method and system |
Non-Patent Citations (1)
| Title |
|---|
| 陈春旭;漆钰晖;朱一帆;裴凌;徐昌庆;: "ICP配准算法的影响因素及评价指标分析" * |
Cited By (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111707262B (en) * | 2020-05-19 | 2022-05-27 | 上海有个机器人有限公司 | Point cloud matching method, medium, terminal and device based on closest point vector projection |
| CN111707262A (en) * | 2020-05-19 | 2020-09-25 | 上海有个机器人有限公司 | Point cloud matching method, medium, terminal and device based on closest point vector projection |
| CN111707279B (en) * | 2020-05-19 | 2023-09-08 | 上海有个机器人有限公司 | Matching evaluation method, medium, terminal and device for laser point cloud and map |
| CN111707279A (en) * | 2020-05-19 | 2020-09-25 | 上海有个机器人有限公司 | Matching evaluation method, medium, terminal and device of laser point cloud and map |
| CN111765882A (en) * | 2020-06-18 | 2020-10-13 | 浙江大华技术股份有限公司 | Laser radar positioning method and related device thereof |
| CN111895989A (en) * | 2020-06-24 | 2020-11-06 | 浙江大华技术股份有限公司 | Robot positioning method and device and electronic equipment |
| CN111812613A (en) * | 2020-08-06 | 2020-10-23 | 常州市贝叶斯智能科技有限公司 | Mobile robot positioning monitoring method, device, equipment and medium |
| CN112462385A (en) * | 2020-10-21 | 2021-03-09 | 南开大学 | Map splicing and positioning method based on laser radar under outdoor large environment |
| CN112258618B (en) * | 2020-11-04 | 2021-05-14 | 中国科学院空天信息创新研究院 | Semantic mapping and positioning method based on fusion of prior laser point cloud and depth map |
| CN112258618A (en) * | 2020-11-04 | 2021-01-22 | 中国科学院空天信息创新研究院 | Semantic mapping and positioning method based on fusion of prior laser point cloud and depth map |
| CN112710312A (en) * | 2020-12-24 | 2021-04-27 | 哈尔滨工业大学芜湖机器人产业技术研究院 | Laser SLAM front-end matching method fusing distance residual and probability residual |
| CN112710312B (en) * | 2020-12-24 | 2023-12-01 | 长三角哈特机器人产业技术研究院 | Laser SLAM front end matching method integrating distance residual error and probability residual error |
| CN112634260A (en) * | 2020-12-31 | 2021-04-09 | 上海商汤智能科技有限公司 | Map evaluation method and device, electronic equipment and storage medium |
| CN113029137A (en) * | 2021-04-01 | 2021-06-25 | 清华大学 | Multi-source information self-adaptive fusion positioning method and system |
| CN113029137B (en) * | 2021-04-01 | 2023-02-28 | 清华大学 | Multi-source information self-adaptive fusion positioning method and system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110969649B (en) | 2024-04-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110969649B (en) | Matching evaluation method, medium, terminal and device for laser point cloud and map | |
| CN111707279B (en) | Matching evaluation method, medium, terminal and device for laser point cloud and map | |
| US10852139B2 (en) | Positioning method, positioning device, and robot | |
| CN110927740B (en) | Mobile robot positioning method | |
| CN108871353B (en) | Road network map generation method, system, equipment and storage medium | |
| CN108732582B (en) | Vehicle positioning method and device | |
| US20210063577A1 (en) | Robot relocalization method and apparatus and robot using the same | |
| CN111079801B (en) | Method, medium, terminal and device for quickly searching closest point based on point cloud matching | |
| CN112595323A (en) | Robot and drawing establishing method and device thereof | |
| CN111707262B (en) | Point cloud matching method, medium, terminal and device based on closest point vector projection | |
| CN116559928B (en) | Pose information determining method, device and equipment of laser radar and storage medium | |
| CN114926549B (en) | Three-dimensional point cloud processing method, device, equipment and storage medium | |
| CN112819199A (en) | Precipitation prediction method, device, equipment and storage medium | |
| CN112198878A (en) | Instant map construction method and device, robot and storage medium | |
| US11034028B2 (en) | Pose determining method for mobile robot and apparatus and mobile robot thereof | |
| CN112097772B (en) | Robot and map construction method and device thereof | |
| CN112154429A (en) | High-precision map positioning method, system, platform and computer readable storage medium | |
| CN110887490A (en) | Key frame selection method, medium, terminal and device for laser positioning navigation | |
| CN113252023A (en) | Positioning method, device and equipment based on odometer | |
| CN115388878A (en) | Map construction method and device and terminal equipment | |
| CN114910892A (en) | Laser radar calibration method and device, electronic equipment and storage medium | |
| CN116295466A (en) | Map generation method, map generation device, electronic device, storage medium and vehicle | |
| CN109919998B (en) | Satellite attitude determination method and device and terminal equipment | |
| CN114677284A (en) | Map construction method and device, electronic equipment and storage medium | |
| CN114998561B (en) | Category-level pose optimization method and device |
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 |