Octree map hole completion method based on class gray level image
Technical Field
The invention relates to the technical field of robot environment perception, in particular to an octree map cavity completion method based on a gray-level-like image.
Background
With the rapid development of computer technology, the research of robots is in depth and the demand of people for robots is expanding, and robots capable of autonomous navigation and intelligent movement become the focus and key point of research. The map is used as one of the bases for autonomous positioning, obstacle avoidance and route planning of the robot, and the importance degree of the map is self-evident.
There are many expressions of maps, such as feature point maps, grid maps, topological maps, and so on. At present, most of robots use point cloud maps in mapping systems, and have some obvious defects: the map form is not compact, the way to handle the overlap is not good enough, and it is difficult to use for navigation. Octree maps are often used for collision detection, path planning, positioning and navigation, and other application tasks, and compared with other forms of maps, Octree maps focus more on modeling a three-dimensional space environment, and store maps using a data structure of Octree (Octree), so that maps can be compressed and updated elegantly, and resolution is adjustable.
Because the truth value of the map is difficult to obtain, the qualitative evaluation of the map constructed by the robot is difficult, and the related research facing the map is few. Due to factors such as sight shielding, sensor visual angle blind areas and the like in the moving process of the mobile robot, a plurality of holes exist in the map building process, and great inconvenience is brought to autonomous navigation and environment understanding of the robot.
Disclosure of Invention
The invention aims to solve the problem of cavities in the octree map building process of a robot, and provides a octree map cavity completion method of gray-level-like images, so that a map with complete information is provided for a mobile robot.
The purpose of the invention is realized by the following technical scheme:
an octree map hole completion method based on a gray-level-like image comprises the following steps:
the method comprises the following steps: reading an octree map;
step two: extracting a three-dimensional coordinate of a central point of each grid in the octree map to form a coordinate set, projecting the central point of each grid to a top-view plane under a world coordinate system, complementing height information of isolated map void points by using a bilinear interpolation method, adding the complemented map void points into the coordinate set, and converting the updated coordinate set into a two-dimensional gray image;
step three: repairing the two-dimensional gray image pixel abnormal region based on a Criminisi algorithm optimized by a Canny operator and a gradient operator, and determining the spatial positions of the central points of all grids in the block-shaped cavity region to be repaired corresponding to the pixel abnormal region;
step four: and storing the spatial positions of the map hole points and all the grid central points of the block-shaped hole area to be complemented to an octree map so as to realize map complementing.
Further, the second step is realized by the following steps:
(2.1) extracting a three-dimensional coordinate of a central point of each grid in the octree map to form a coordinate set, projecting the central point to a top view plane (xoy plane) in a world coordinate system, setting the height of an isolated map cavity point in the xoy plane as an abnormal value, and extracting the maximum height value of each coordinate point of the projected plane;
(2.2) complementing the height information of the isolated map cavity points in the xoy plane by using a bilinear interpolation method, adding the complemented isolated cavity points into a coordinate set, and updating the coordinate set;
(2.3) zooming and translating the x value and the y value of the coordinate point in the updated coordinate set according to the boundary size of the map and the resolution of the octree map, namely respectively transforming the minimum value of the x value and the y value to the pixel coordinate origin, transforming the coordinate points with the resolution of the octree map as an interval into continuous pixel coordinate points, and transforming the height z value corresponding to the coordinate points into the pixel value of the corresponding pixel point, thereby transforming the coordinate set into a two-dimensional gray image.
Further, the third step is realized by the following steps:
(3.1) determining a corresponding block-shaped cavity area to be compensated and a boundary thereof according to the pixel abnormal area in the two-dimensional gray level image corresponding to the abnormal value;
(3.2) optimizing a priority function by combining a Canny operator and a gradient operator, balancing the influence of confidence and data items on the priority, calculating the priority of the edge pixel, finding a target block with the maximum priority, searching a known pixel block which is most matched with the target block by using a Criminisi algorithm, repairing the target block, and finishing the updating of the abnormal pixel value in the gray level image;
(3.4) screening the updated abnormal pixel points of the two-dimensional gray image, re-determining the boundaries of the abnormal pixel points, and repeating the steps (3.2) and (3.3) until the updating of the pixel values of all the abnormal pixel points is completed;
and (3.5) determining the pixel values of all abnormal pixel points according to the updated values, converting the continuous pixel coordinate points into x and y coordinate values through inverse transformation, and converting the pixel values of the corresponding pixel points into height values z, thereby determining the three-dimensional space positions of all map completion points.
Further, the calculation formula of the height information of the map hole point is as follows:
wherein HaIs an isolated map hole point (x)a,ya) Height of (H)11、H21、H12And H22Respectively are coordinate points (x) next to the four positive directions of the isolated hole point1,y1)、(x2,y1)、(x1,y2) And (x)2,y2) Corresponding heightInformation; selecting coordinate points which are close to four positive directions of the isolated hole point, namely selecting points which are parallel to the x direction and the y direction and are separated from the hole point of the map by one octree map resolution ratio, namely x1=xa-0.5,x2=xa+0.5,y1=ya-0.5,y2=ya+0.5, thereby ensuring that the screened map hole point is an isolated single point.
Further, the Canny operator and the gradient operator are combined to optimize a priority function of
P(p)=αC(p)+βD(p)+γLG(p)
Wherein, the coefficients α, β, y are weight factors adjusted according to the texture features of different images, and c (p) is a confidence term in the Criminisi algorithm, which is the ratio of the sum of known information in the pixel block to be repaired to the area of the pixel block to be repaired; d (p) is a data item in the Criminisi algorithm and represents the structural information in the pixel block to be repaired; LG (x, y) is a structural term,
LG(x,y)=|dx(i,j)|+|dy(i,j)|
dx(i,j)=Canny(i+1,j)-Canny(i,j);
dy(i,j)=Canny(i,j+1)-Canny(i,j);
dx (i, j) and dy (i, j) represent the amount of change in Canny value in the x direction and the y direction, respectively, of the point (i, j).
The invention has the following beneficial effects:
(1) the method of the invention utilizes a bilinear interpolation method to fill isolated void points, and utilizes a gray-like image method to fill block-shaped void areas. The method can complement the map by combining the characteristics of the linear structure, the two-dimensional texture and the like of the environment, so that a more complete map is obtained, the decision of the robot is facilitated, and the robot is guided to carry out path planning better.
(2) Due to the fact that the texture characteristics and the structural characteristics of the earth surface environment of the extraterrestrial celestial sphere are rich, the extraterrestrial celestial sphere detection method can be well used for the extraterrestrial planet detection process and used for the extraterrestrial planet detection vehicle to conduct environment understanding and path planning, and therefore detection efficiency of the detection vehicle is improved.
Drawings
Fig. 1 is a flowchart of an octree map void completion method based on a class gray image according to this embodiment;
fig. 2 is a structure diagram of the Criminisi algorithm optimized based on the Canny operator and the gradient operator in this embodiment.
Detailed Description
The present invention will be described in detail below with reference to the accompanying drawings and preferred embodiments, and the objects and effects of the present invention will become more apparent, it being understood that the specific embodiments described herein are merely illustrative of the present invention and are not intended to limit the present invention.
The octree map void completion method based on the class gray level image, disclosed by the invention, has a flow chart shown in figure 1, and comprises the following steps:
step one, reading an octree map.
An octree map is made up of many small squares, each square representing the probability that the cell is occupied. When the resolution is high, the square is small; at lower resolutions, the square is large. Specifically, the resolution of the octree map is set to 0.5 in this embodiment, and a suitable resolution can be set for a specific probe vehicle and a specific probe task in the specific task process;
extracting three-dimensional coordinates of the central point of each grid in the octree map to form a coordinate set, projecting the central point of each grid to a top-view plane under a world coordinate system, complementing the height information of the isolated void points by using a bilinear interpolation method, adding the complemented isolated void points into the coordinate set, and converting the updated coordinate set into a two-dimensional gray image;
(2.1) inquiring the center coordinate of each leaf node in the octree map by using a leaf _ iterator batch inquiry iterator and a getCoordinate () function, thereby extracting the three-dimensional coordinate of each grid center point in the octree map, and projecting each grid center point to a xoy plane, wherein the maximum height value of the grid center point corresponding to each projection point is extracted as the height information of the projection point because a plurality of grid center points in the octree map are likely to correspond to the same projection point after being projected to the xoy plane; and defining points without height values in the xoy plane as map hole points, and setting the height of the map hole points as abnormal high values. In specific implementation, the height of the map void point can be set as an abnormal value, so that the map void point can be screened from the octree map. In this embodiment, the abnormal value is set to 100.
(2.2) complementing the height information of the isolated map cavity points in the xoy plane by using a bilinear interpolation method, adding the complemented isolated cavity points into a coordinate set, and updating the coordinate set;
when the coordinates in four positive directions next to the hole point (with the resolution of the octree map as an interval) correspond to the height value, the hole point is defined as an isolated map hole point. And calculating the height information of the isolated hole points of the map by using the height information corresponding to the coordinates in the four positive directions adjacent to the hole points through a bilinear interpolation method:
wherein HaIs an isolated map hole point (x)a,ya) Height of (H)11、H21、H12And H22Respectively are coordinate points (x) next to the four positive directions of the isolated hole point1,y1)、(x2,y1)、(x1,y2) And (x)2,y2) Corresponding height information. Selecting coordinate points which are close to four positive directions of the isolated hole point, namely selecting points which are parallel to the x direction and the y direction and are separated from the hole point of the map by one octree map resolution ratio, namely x1=xa-0.5,x2=xa+0.5,y1=ya-0.5,y2=ya+0.5, thereby ensuring that the screened map hole point is an isolated single point.
(2.3) zooming and translating the x value and the y value of the coordinate point in the updated coordinate set according to the boundary size of the map and the resolution of the octree map, namely respectively transforming the minimum value of the x value and the y value to the pixel coordinate origin, transforming the coordinate points with the resolution of the octree map as an interval into continuous pixel coordinate points, and transforming the height z value corresponding to the coordinate points into the pixel value of the corresponding pixel point, thereby transforming the coordinate set into a two-dimensional gray image.
And thirdly, repairing the pixel abnormal region of the two-dimensional gray image based on a Criminisi algorithm optimized by a Canny operator and a gradient operator, and determining the spatial positions of all grid center points of the block-shaped cavity region to be repaired corresponding to the pixel abnormal region.
The Criminisi algorithm is an image restoration method based on samples, and the main work is to continuously select the best matching block from a known region and fill the best matching block into a region to be restored from outside to inside. The method has the advantages that places with more complete areas and stronger structures around the boundary of the cavity are repaired preferentially, filling accuracy is guaranteed to the maximum extent through priority calculation, the accuracy is transmitted orderly, and the fuzzy phenomenon generated when large-size damaged images are repaired can be avoided.
(3.1) determining a corresponding block-shaped cavity area to be compensated and a boundary thereof according to the pixel abnormal area in the two-dimensional gray level image corresponding to the abnormal value set in the step two;
(3.2) combining a Canny operator and a gradient operator to optimize a priority function, calculating the priority of the edge pixels, finding out a target block with the maximum priority, searching a known pixel block which is most matched with the target block by using a Criminisi algorithm, repairing the target block, and finishing the updating of the abnormal pixel value in the gray level image;
the priority of point p at the boundary of the void region in the Criminisi algorithm is:
P(p)=C(p)×D(p)
wherein, C (p) is a confidence term and is the ratio of the sum of the known information in the pixel block to be repaired to the area of the pixel block to be repaired, and D (p) is a data term and represents the structural information in the pixel block to be repaired. The priority function is performed in a multiplicative fashion, and as repair work progresses, any decrease in the confidence term or data term causes the priority value to decrease rapidly. In the subsequent repair process, the size of the priority value cannot correctly reflect the repair sequence of the block to be repaired.
The gradient calculation formula of a general image is as follows:
where I is the entire image, G (x, y) is the gradient at point (x, y), and dx and dy are the gradient values in the x and y directions, respectively. In most cases, the repair order does not depend only on the boundary information of the image, but also the texture information plays a very important role. The Canny operator is simple in calculation, can identify actual edges in the image, and is sensitive to the edges of the image, but the edges in the image can be identified only once, and possible image noise can be identified as the edges, but the extraction efficiency of the complex image is not high, so that the accurate repair sequence cannot be completely obtained by using the Canny characteristic or the gradient characteristic alone. Aiming at the problems, the Canny operator is applied to gradient operation, a structural item based on the combination of the Canny operator and the gradient operator is provided, and is merged into the priority in an adding mode, and the specific definition is as follows:
dx (i, j) and dy (i, j) represent Canny value variations of the point (i, j) in the x direction and the y direction respectively, and can well describe texture information around the sample block and reflect information of the block edge. The priority function in the original Criminisi algorithm is in a product form, so that the problem that the repair sequence is unreliable due to zero priority value easily occurs, and the priority function in the Criminisi algorithm is defined in a weighted sum form, so that the problems can be avoided, and the influence of confidence and data items on the priority can be balanced. Finally, the improved priority formula is as follows:
P(p)=αC(p)+βD(p)+γLG(p)
wherein the coefficients α, β, γ need to be adjusted according to the texture features of different images.
When the gradient at the central pixel point p is large and the surrounding texture is rich, the larger the constraint term lg (p), the larger the sample block priority centered on p, and vice versa.
(3.4) screening the updated abnormal pixel points of the two-dimensional gray image, re-determining the boundaries of the abnormal pixel points, and repeating the steps (3.2) and (3.3) until the updating of the pixel values of all the abnormal pixel points is completed;
and (3.5) determining the pixel values of all abnormal pixel points according to the updated values, converting the continuous pixel coordinate points into x and y coordinate values through inverse transformation, and converting the pixel values of the corresponding pixel points into height values z, thereby determining the three-dimensional space positions of all map completion points.
And fourthly, storing the spatial positions of the map hole points and all the grid central points of the block-shaped hole area to be supplemented to the octree map to complete the map supplementation.
The invention is used for complementing the outline of the missing area in the octree map, can display whether barriers exist at a certain height in the space map, and does not influence the functionality of the navigation map. In practical application, a certain threshold value can be set between the ground and the outer contour for completion according to the requirements of a practical scene, so that a complete three-dimensional map is obtained.
It will be understood by those skilled in the art that the foregoing is only a preferred embodiment of the present invention, and is not intended to limit the invention, and although the invention has been described in detail with reference to the foregoing examples, it will be apparent to those skilled in the art that various changes in the form and details of the embodiments may be made and equivalents may be substituted for elements thereof. All modifications, equivalents and the like which come within the spirit and principle of the invention are intended to be included within the scope of the invention.