A multi-feature pose estimation method for cross-view scenes

Abstract

The application belongs to the field of multi-agent collaborative perception, and specifically discloses a multi-feature pose estimation method for cross-view scenes, which comprises the following steps: in the mapping process, the unmanned aerial vehicle and the unmanned vehicle respectively acquire image information through binocular cameras, and respectively extract point features and line features in the images, and then find point-line combinations through an inflation algorithm; in the single-agent positioning process, the point-line combinations are used to constrain the pose estimation process; in the multi-agent exchange map information process, the point cloud is preferentially coarsely registered through a PCM algorithm combined with the point-line combinations; finally, a distributed RANSAC algorithm is constructed through an octree map, so that high-precision multi-agent relative pose estimation is realized, and accurate relative pose estimation is provided for subsequent multi-agent collaborative mapping and collaborative planning.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of multi-agent collaborative perception, and in particular to a multi-feature pose estimation method for cross-view scenes. BACKGROUND

[0002] In recent years, with the rapid development of unmanned system technology, multi-agent collaborative work has been widely applied in various fields. Cross-view air-ground heterogeneous collaborative positioning is an innovative method to achieve efficient collaborative work in multi-agent systems. In the context of today's technological development, unmanned aerial vehicles and unmanned ground vehicles are increasingly used in air-ground heterogeneous multi-agent systems. These multi-agents can better adapt to different task requirements through collaborative work.

[0003] During the collaborative positioning process, the collaborative work of unmanned aerial vehicles and unmanned ground vehicles faces a series of challenges. One of these challenges is that due to the influence of cross-view, the environmental information obtained by the unmanned aerial vehicle and the unmanned ground vehicle when observing the same scene is significantly different. Moreover, considering the cross-domain collaborative process, both the unmanned ground vehicle and the unmanned aerial vehicle store a large amount of map information before interacting, which causes a significant time consumption for mutual map estimation. SUMMARY

[0004] To address the above deficiencies and / or improve improvements of prior art, a multi-feature pose estimation method for cross-view scenes is provided, which aims to reduce the multi-agent mutual pose estimation error caused by cross-view and improve the efficiency and accuracy of mutual pose estimation of agents under cross-view.

[0005] To achieve the above purpose, according to one aspect of the present application, a multi-feature pose estimation method for cross-view scenes is provided, comprising the following steps:

[0006] S1: During the single-agent positioning process, point features and line features are extracted from the image information in the binocular camera;

[0007] S2: The single agent integrates the point feature and line feature information obtained in step S1 and extracts point-line combinations through the inflation algorithm;

[0008] S3: Based on step S2, if there are related point-line combinations between two frames during the positioning process of the single agent, the single agent constructs an inter-frame pose estimation cost function by combining point features, line features, and point-line combinations to perform inter-frame pose estimation;

[0009] S4: An octree map is constructed for the point-line related features obtained by the single agent in steps S1-S3, and the point-line combination three-dimensional coordinate information, line feature and point feature related information, and octree related node information are transmitted during the multi-agent interaction process;

[0010] S5: Based on the step S4 interaction process, the point-line combination is used to implement the pose rough estimation and octree grid alignment through the PCM algorithm;

[0011] S6: After completing the octree grid alignment in step S5, the distributed RANSAC algorithm is executed, the octree nxn grid is used as the basic operator, the related matching information is extracted by extracting the parent node form, and the pose optimization operation is performed inside the basic operator;

[0012] S7: The relative pose estimation calculated in each basic operator obtained in step S6 is extracted to form a relative pose estimation set, and the RANSAC processing is further performed according to the set, so as to realize the high-precision multi-agent relative pose estimation.

[0013] As preferred, the point feature is composed of the FAST corner point and the MobileNetVLAD descriptor, and the point feature is uniformly extracted in step S1.

[0014] The line feature is extracted by using the LSD feature, which is represented as the first and last two points of the line segment and the LBD descriptor, and the length screening extraction is performed on the line feature in step S1.

[0015] As preferred, the specific steps of the inflation algorithm in step S2 are as follows: the end points of the line segment are randomly selected in a frame of picture, the surrounding is searched and the nearest point is found, the included angle between the direction of the other end point of the nearest point and the current straight line is solved, if the included angle is greater than the designed threshold value, the two line segments are the broken line, and the two line segments are not merged; finally, the distance between the far point of the current line and the end points of the nearest line is solved, if the far distance is less than the near distance, the two line segments are the same direction adjacent line segments, and the longer line feature is taken.

[0016] As preferred, the specific formula of the inflation algorithm in step S2 is as follows:

[0017]

[0018] Wherein, r is the search radius, σ max is the search radius threshold, θ max is the direction angle threshold, p ou represents the pixel coordinates of the u-th point in the o-th line segment, p o1 is the pixel coordinates of the 1st point in the o-th line segment, p p2 is the pixel coordinates of the 2nd point in the p-th line segment.

[0019] As preferred, the out points in the point feature or the line feature are removed according to the point-line combination before the estimation in step S3, and the specific formula of the cost function is as follows:

[0020]

[0021] wherein is the Lie group representation of camera pose, and the following are all represented by Lie group, b represents the bth feature, k represents the total number of corresponding features, and b p represents the bth point feature, k p represents the total number of point features; u p is the pixel coordinate of the point feature projection, is the vector value of the line feature projection, u pl is the pixel coordinate of the point-line combination projection, K is the camera intrinsic matrix, which corresponds to the mapping from the three-dimensional coordinates of objects in the environment to the camera pixel coordinate system; P p is the three-dimensional coordinate of the feature point in the environment, is the three-dimensional vector of the line feature in the environment, P pl is the three-dimensional coordinate of the point-line feature point in the environment.

[0022] As preferred, step S5 specifically includes the following steps:

[0023] S51: According to the angle information of multiple line segments of the point-line combination and the three-dimensional coordinates of the point as constraint conditions, optimization is performed, and cosine distance is used for comparison to check the consistency of the data;

[0024] S52: Align the grid networks with the first and second largest similarities in the octree grid.

[0025] As preferred, the specific formula in step S51 is:

[0026]

[0027] wherein i and j respectively represent the ith and jth intelligent agent that generates interaction, e represents the calculation error, and the same parameters below represent the same.

[0028] Further, represents the optimal relative pose estimation obtained by the PCM algorithm; n and m represent the number of aerial unmanned vehicles and ground unmanned vehicles; p and q respectively represent the three-dimensional coordinate sets of the points matched in the maps of the two intelligent agents; represents the line segment vector set; w and q respectively represent the wth and qth line feature vector information sets in the corresponding intelligent agents.

[0029] As preferred, the specific formula in step S6 is:

[0030]

[0031] wherein, represents the optimal estimated pose calculated by the child node grid; p and q respectively represent the three-dimensional coordinate set of the matching points in the two agent maps; K represents the camera intrinsic matrix, R represents the rotation in the pose estimation, t represents the translation matrix in the calculated 4x4 transformation matrix, d and m represent the related parameters in the Plucker coordinate system, represents d=y-x, m=x*y, x and y are the horizontal and vertical coordinates in the Plucker coordinate system respectively; β respectively represent the point-line feature calculation weights.

[0032] Preferably, the specific formula of RANSAC in step S7 is as follows:

[0033]

[0034] wherein, represents the optimal pose estimation calculated by the parent node on the child node RANSAC calculation.

[0035] Preferably, when multiple agents cooperate, the agents are unmanned vehicles or unmanned aerial vehicles, and each unmanned vehicle and / or unmanned aerial vehicle respectively performs steps S1-S3 when no interaction occurs, and multiple unmanned aerial vehicles and / or unmanned vehicles jointly perform steps S5-S7 when interaction occurs.

[0036] Overall, compared with the prior art, the above technical solutions conceived by the present application have the following beneficial effects:

[0037] 1. Reducing the multi-agent mutual pose estimation error problem caused by cross-view, improving the efficiency and accuracy of cross-view agent mutual pose estimation;

[0038] 2. By constructing an octree map, during the cross-domain collaboration process, the unmanned vehicles and unmanned aerial vehicles transfer the point-line combination three-dimensional coordinate information, line feature and point feature related information, and octree related node information during the interaction process, saving the time consumption caused by mutual estimation of the map, and improving the efficiency and accuracy of cross-view agent mutual pose estimation. BRIEF DESCRIPTION OF DRAWINGS

[0039] Figure 1 It is a wide-area search cross-view collaborative perception system schematic diagram for the embodiment of the present application;

[0040] Figure 2 It is a point-line combination pose estimation principle schematic diagram for the embodiment of the present application;

[0041] Figure 3 It is a distributed RANSAC point cloud matching principle schematic diagram for the embodiment of the present application.

[0042] Figure 4A cross-view multi-feature pose estimation process schematic diagram for the embodiment of the present application. DETAILED DESCRIPTION

[0043] In order to make the purpose, technical solutions and advantages of the present application clearer and easier to understand, the present application will be described in more detail in conjunction with the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application, not to limit the present application. In addition, the technical features mentioned in each embodiment of the present application can be combined with each other as long as there is no conflict between them.

[0044] The multi-feature pose estimation method for the cross-view scene provided by the embodiment of the present application is as shown in the figure. Figure 1 The main bodies for cooperative positioning are unmanned aerial vehicles and unmanned vehicles. Both of them are equipped with camera sensors to obtain image information in the environment. The whole system is divided into single machine perception and cooperative perception two parts.

[0045] In the process of multi-agent cooperative positioning, if there is no interaction between agents, each agent performs positioning function. If the remaining agents are discovered through UWB, the relevant map information is transmitted. In this embodiment, UWB is only used for communication processing and does not have positioning function.

[0046] For convenience of description, the unmanned aerial vehicle and the unmanned vehicle are collectively referred to as an agent in this embodiment.

[0047] The cross-view cooperative positioning based on the above architecture includes the following steps:

[0048] S1: In the process of single-agent positioning, the image information in the binocular camera is extracted for point feature and line feature; the point feature is composed of FAST corner point and MobileNetVLAD descriptor, and the line feature is extracted by using LSD feature, which is represented as the first and last two points of the line segment and LBD descriptor. The point feature is uniformly extracted, and the line feature is extracted by length screening.

[0049] S2: The single agent integrates the point feature and line feature information obtained in step S1 and extracts the point-line combination through the inflation algorithm. The specific steps of the inflation algorithm are as follows: randomly selecting the end point of the line segment in a frame of picture, searching around it and finding the nearest point, solving the included angle between the direction of the other end point of the nearest point and the current straight line, if the included angle is greater than the designed threshold value, the two line segments are polyline, and they are not merged; finally, solving the distance between the far point of the current line and the end points of the nearest line, if the far distance is less than the near distance, the two line segments are adjacent line segments in the same direction, and the longer line feature is taken. The specific formula can be expressed as:

[0050]

[0051] Wherein, r is the search radius, and σmax θ is the search radius threshold. max p is the threshold for the direction angle. ou p represents the pixel coordinates of the u-th point in the o-th line segment. o1 Let p be the pixel coordinates of the first point in the o-th line segment. p2 Let be the pixel coordinates of the second point in the p-th line segment.

[0052] Furthermore, the found point-line combination should have the following information: pixel coordinates, relevant line LBD descriptor, and relevant MobileNetVLAD descriptor.

[0053] Specifically, considering that point-line composites have strong geometric properties compared to other single features, whether in cross-view or normal odometry situations, using this as the basis for pose estimation can reduce computation time while improving pose estimation accuracy.

[0054] S3: Based on step S2, if, ideally, the agent has a related point-line combination in two frames during localization, then the point-line combination is selected as the initial pose estimation basis during motion. For example... Figure 2 As shown, in environments with strong geometric features such as buildings and roads, such as cities and rural areas, there exists a certain point-line combination. These features are mostly static and clearly distinguishable from their surroundings. Therefore, in the pose estimation process, a constraint term is added around the point-line combination, with the specific expression as follows:

[0055]

[0056] in, Li Qun, referring to the camera's pose, stated that the following is based on... All relevant parameters are represented by a Lie group, where b represents the b-th feature and k represents the total number of corresponding features. p Let k represent the feature of the b-th point. p Represented as the total number of point features; u p These are the pixel coordinates of the point feature projection. u is the vector value of the line feature projection. pl P represents the pixel coordinates of the projection of the point-line combination volume; K is the camera intrinsic parameter matrix, which corresponds to the mapping from the 3D coordinates of objects in the environment to the camera pixel coordinate system; p These are the three-dimensional coordinates of the feature points in the environment. P is a three-dimensional vector of line features in the environment. pl These are the three-dimensional coordinates of the point / line feature points in the environment.

[0057] Further, in the pose calculation process, the error of the point-line combination is calculated to detect the error of the point and line features in the environment. The point and line features with large deviations are removed as dynamic features, thereby reducing the noise caused by environmental information.

[0058] S4: Construct an octree map for the point-line related features in the environment obtained by the single agent in steps S1-S3. The information exchanged during the interaction of the agents includes the three-dimensional coordinates of the point-line combination, the related information of the line and point features, and the related node information of the octree.

[0059] S5: Based on the interaction process in step S4, the PCM algorithm is performed by the point-line combination to realize pose estimation and octree grid alignment.

[0060] Specifically, the angle information of the multiple line segments of the point-line combination and the three-dimensional coordinates of the points are used as constraint conditions for optimization. For consistency checking, the cosine distance is used for comparison. The specific formula is:

[0061]

[0062] where i and j represent the ith and jth agents that interact, e represents the calculation error, represents the optimal relative pose estimation obtained by the PCM algorithm; n and m represent the number of unmanned aerial vehicles and unmanned ground vehicles; p and q represent the three-dimensional coordinate sets of the points matched in the maps of the two agents; represents the line vector set; w and q represent the line feature vector information sets of the wth and qth agents, respectively.

[0063] Further, for the first and second largest grid networks in the aligned octree grid.

[0064] S6: After completing the octree grid alignment in step S5, the distributed RANSAC algorithm is executed, as shown in Figure 3 The octree nxn grid is used as a basic operator, and the related matching information is extracted by extracting the parent node form to perform pose optimization operation on the basic operator. The specific formula is as follows:

[0065]

[0066] where represents the optimal estimated pose calculated by the child node grid; p and q respectively represent the three-dimensional coordinate set of the matching points in the two agent maps; K represents the camera intrinsic matrix, R represents the rotation in the pose estimation, t represents the translation matrix in the calculated 4x4 transformation matrix, d and m represent the related parameters in the Plucker coordinate system, d=y-x, m=x*y, x and y are the horizontal and vertical coordinates in the Plucker coordinate system, respectively; β respectively represent the point-line feature calculation weights.

[0067] S7: Extract the relative pose estimation calculated in each base operator obtained in step S6 to form a relative pose estimation set, and further perform RANSAC processing according to the set, so as to realize high-precision multi-agent relative pose estimation.

[0068] Extract the relative pose estimation extracted by each grid to form a pose set, and further perform RANSAC processing according to the set. The specific formula is as follows

[0069]

[0070] Wherein, represents the optimal pose estimation obtained by the parent node for the RANSAC calculation of the child node pose.

[0071] When multiple agents cooperate, the agent is an unmanned vehicle or a drone. When no interaction occurs, each unmanned vehicle and / or drone performs steps S1-S3. When interaction occurs, multiple drones and / or unmanned vehicles jointly perform steps S5-S7. Considering that the environment information is screened twice in the matching process, and the huge map information is calculated in the form of a distributed operator, both the estimation accuracy and the required calculation cost can be improved.

[0072] Although the present application focuses on the air-ground collaborative heterogeneous relative pose estimation method, this method still has certain reference significance for isomorphic multi-agent collaborative mapping. That is, by combining simple features to construct complex features, high-precision estimation can be achieved, and there is a certain deviation between different agents observing from different angles.

[0073] Those skilled in the art can easily understand that the above is only a preferred embodiment of the present application, and cannot be regarded as a limitation on the present application. Any modification, equivalent replacement or improvement made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A multi-feature pose estimation method for cross-view scenes, characterized in that, Comprise the following steps: S1: in the single agent positioning process, the image information in the binocular camera is extracted from the point feature and the line feature; S2: the single agent integrates the point feature and the line feature information obtained in step S1 and extracts the point-line combination through the inflation algorithm; S3: on the basis of step S2, if there are related point-line combinations in two frames during the positioning process of the single agent, the single agent estimates the inter-frame pose by constructing a cost function of inter-frame pose estimation through the point feature, the line feature and the point-line combination; before estimation, the outliers in the point feature or the line feature are removed according to the point-line combination, and the specific formula of the cost function is: wherein, is a Lie group representation of camera pose, is a pixel coordinate of point feature projection, is a vector value of line feature projection, is a pixel coordinate of point-line combined feature projection, is a camera intrinsic matrix, which corresponds to a mapping from three-dimensional coordinates of objects in the environment to the camera pixel coordinate system; is a three-dimensional coordinate of a point feature in the environment, is a three-dimensional vector of a line feature in the environment, is a three-dimensional coordinate of a point-line feature in the environment; S4: the octree map is constructed for the point feature, the line feature and the point-line combination of the environment obtained by the single agent in steps S1-S3, and the three-dimensional coordinate information of the point-line combination, the related information of the line feature and the point feature and the related node information of the octree are transmitted in the multi-agent interaction process; S5: based on the interaction process in step S4, the PCM algorithm is performed through the point-line combination to realize the pose rough estimation and the octree grid alignment; S6: after the octree grid alignment in step S5 is completed, the distributed RANSAC algorithm is executed, the n*n grids of the octree are taken as the basic operators, the related matching information is extracted by extracting the parent node form, and the pose optimization operation is performed on the basic operators; S7: the relative pose estimation calculated in each basic operator obtained in step S6 is extracted to form a relative pose estimation set, and the RANSAC processing is further performed according to the set, so that the high-precision multi-agent relative pose estimation is realized.

2. The multi-feature pose estimation method for cross-view scenes of claim 1, wherein, The point feature is composed of the FAST corner point and the MobileNetVLAD descriptor, and the point feature is uniformly extracted in step S1; The line feature is extracted by using the LSD feature, which is represented as the first and last two points of the line segment and the LBD descriptor, and the line feature is extracted by length screening in step S1.

3. The multi-feature pose estimation method for cross-view scenes of claim 1, wherein, The specific steps of the inflation algorithm in step S2 are as follows: randomly selecting the end points of the line segment in a frame, searching around the end points and finding the nearest point, solving the included angle between the direction of the other end point of the nearest point and the current straight line, if the included angle is greater than the designed threshold, the two line segments are fold lines, and they are not merged; finally, the distance between the far point of the current line and the end points of the nearest line is solved, if the far distance is less than the near distance, the two line segments are adjacent line segments in the same direction, and the longer line feature is taken.

4. The multi-feature pose estimation method for cross-view scenes of claim 3, wherein, The specific formula of the inflation algorithm in step S2 is: wherein, r is a search radius, is a search radius threshold, is a direction angle threshold, is a pixel coordinate of a first point in the i-th line segment, is a pixel coordinate of a second point in the i-th line segment, is a pixel coordinate of a first point in the j-th line segment, is a pixel coordinate of a second point in the j-th line segment, denotes a direction vector of the i-th line segment.

5. The multi-feature pose estimation method for cross-view scenes of claim 1, wherein, Step S5 specifically comprises the following steps: S51: the consistency of the data is tested by using the cosine distance to compare the included angle information of the multiple line segments of the point-line combination and the three-dimensional coordinates of the point as the constraint condition; S52: the grid networks with the first and second largest similarity in the aligned octree grid.

6. The multi-feature pose estimation method for cross-view scenes of claim 5, wherein, The specific formula in step S51 is: in, i , j These represent the first interaction. i The and the first j There are three agents, where e represents the computational error. This represents the optimal relative pose estimate obtained through the PCM algorithm; n , m This indicates the number of aerial drones and ground-based unmanned vehicles; p , q These represent the sets of three-dimensional coordinates of mutually matching points in the maps of the two agents; Represented as a set of line segments and line vectors; w , q They represent the first and second intelligent agents, respectively. w The and the first q A set of linear feature vector information.

7. The multi-feature pose estimation method for cross-view scenes of claim 1, wherein, The specific formula in step S6 is: wherein, represents the best estimated pose computed as a child node grid; p , q represent the sets of three-dimensional coordinates of the mutually matched points in the two agent maps, respectively; K represents the camera intrinsic matrix, R represents the rotation in the pose estimation, t represents the translation matrix in the computed 4x4 transformation matrix, d, m are related parameters in the Plucker coordinate system, and are represented as d= y-x, m=x x y , x , y are the horizontal and vertical coordinates in the Plucker coordinate system, respectively; , represent the point and line feature computation weights, respectively.

8. The multi-feature pose estimation method for cross-view scenes of claim 1, wherein, When the multiple agents cooperate, the agents are unmanned vehicles or unmanned aerial vehicles, and each unmanned vehicle and / or unmanned aerial vehicle respectively performs steps S1-S3 when no interaction occurs, and the multiple unmanned aerial vehicles and / or unmanned vehicles jointly perform steps S5-S7 when interaction occurs.

Images