Multi-feature pose estimation method for cross-view scene

Abstract

The invention belongs to the field of multi-agent collaborative perception, and particularly discloses a cross-view scene-oriented multi-feature pose estimation method, which comprises the following steps that: an unmanned aerial vehicle and an unmanned vehicle acquire image information through binocular cameras respectively in a mapping process, and extract point features and line features in images respectively; a point-line combination body is found through an expansion algorithm; the pose estimation process is constrained through a point-line combination body in the single agent positioning process; in the process of exchanging map information by multiple agents, performing coarse registration on point clouds preferentially by combining a PCM algorithm with a point-line combination body; and 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 later multi-agent collaborative mapping, collaborative planning and the like.

Description

Technical Field

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

[0002] In recent years, with the rapid development of unmanned system technology, multi-agent collaborative work has been widely used in various fields. Cross-perspective 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, the application of air-ground heterogeneous multi-agents such as drones and unmanned vehicles is gradually increasing. These multi-agents can better adapt to different task requirements through collaborative work.

[0003] In the process of collaborative positioning, the cooperation between drones and unmanned vehicles faces a series of challenges. One of them is that due to the influence of cross-perspective, the environmental information obtained by drones and unmanned vehicles when observing the same scene is quite different. And considering the cross-domain collaboration process, unmanned vehicles and drones store a lot of map information before interacting, which consumes a lot of time for mutual estimation of maps. Summary of the invention

[0004] In view of the above defects or improvement needs of the prior art, the present invention provides a multi-feature pose estimation method for cross-view fields, which aims to reduce the error problem of mutual pose estimation of multiple agents caused by cross-view and improve the efficiency and accuracy of mutual pose estimation of agents under cross-view.

[0005] To achieve the above object, according to one aspect of the present invention, a multi-feature pose estimation method for a cross-viewing scene 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 features and line feature information obtained in step S1 and extracts the point-line combination through the expansion algorithm;

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

[0009] S4: Construct an octree map for the point-line related features in the environment obtained by the single agent in steps S1-S3, and transmit the three-dimensional coordinate information of the point-line combination, related information of line features and point features, and octree related node information during the multi-agent interaction;

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

[0011] S6: After completing the octree grid alignment in step S5, execute the distributed RANSAC algorithm, use the octree nxn grids as the basic operator, extract the relevant matching information by extracting the parent node form, and perform pose optimization operations on 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 RANSAC processing is further performed based on the set to achieve high-precision multi-agent relative pose estimation.

[0013] Preferably, the point features are composed of FAST corner points and MobileNetVLAD descriptors, and the point features are uniformly extracted in step S1;

[0014] The line feature is extracted using LSD features, which are represented by the first and last points of the line segment and the LBD descriptor. In step S1, the line feature is length-screened and extracted.

[0015] Preferably, the specific steps of the expansion algorithm in step S2 are to randomly select a line segment endpoint in a frame, search around it and find the nearest point, solve the angle between the direction of the current point at the other end of the line segment of the nearest point and the current straight line. If it is greater than the designed threshold, the two line segments are broken lines and are not merged; finally, solve the distance between the far point of the current line and the two end points of the nearest line. If the far distance is smaller than the short distance, the two line segments are adjacent line segments in the same direction, and the longer line feature is taken.

[0016] Preferably, the specific formula of the expansion algorithm in step S2 is:

[0017]

[0018] Among them, 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 coordinate of the first point in the oth line segment, p p2 is the pixel coordinate of the second point in the pth line segment.

[0019] Preferably, in step S3, external points in point features or line features are eliminated based on the point-line combination before estimation, and the specific formula of the cost function is:

[0020]

[0021] in is the Lie group representation of the camera posture, and the following The relevant parameters are expressed in Lie groups, b represents the bth feature, k represents the total number of corresponding features, then b p Represented as the b-th point feature, k p Expressed as 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 projection of the point-line combination, K is the camera internal parameter matrix, which corresponds to the mapping of the three-dimensional coordinates of the object 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 and line feature points in the environment.

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

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

[0024] S52: Align the grid networks with the highest and second highest similarity among the octree grids.

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

[0026]

[0027] Where i and j represent the i-th and j-th agents that interact, respectively, and e represents the calculation error. The same parameters are expressed in the following way.

[0028] further, It is represented by the optimal relative pose estimation obtained by the PCM algorithm; n and m represent the number of aerial drones and ground unmanned vehicles; p and q represent the three-dimensional coordinate sets of the matching points in the maps of the two agents, respectively; It is represented as a set of line segment vectors; w and q respectively represent the w-th and q-th line feature vector information sets in the corresponding agent.

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

[0030]

[0031] in, It represents the best estimated pose calculated by the child node grid; p and q represent the three-dimensional coordinate sets of the matching points in the two agent maps; K represents the camera intrinsic parameter 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 relevant parameters in the Plucker coordinate system, expressed as d=yx, m=x×y, where x and y are the horizontal and vertical coordinates in the Plucker coordinate system respectively; β represents the weights for calculating point and line features respectively.

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

[0033]

[0034] in, It represents the optimal pose estimate obtained by performing RANSAC calculation on the pose calculated by the parent node for the child node.

[0035] Preferably, when multiple intelligent agents collaborate, the intelligent agents are unmanned vehicles or drones. When no interaction occurs, each unmanned vehicle and / or drone executes steps S1-S3 respectively. When interaction occurs, multiple drones and / or unmanned vehicles jointly execute steps S5-S7.

[0036] In general, the above technical solution conceived by the present invention has the following beneficial effects compared with the prior art:

[0037] 1. Reduce the error in multi-agent mutual pose estimation caused by cross-viewpoints, and improve the efficiency and accuracy of multi-agent mutual pose estimation under cross-viewpoints;

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

[0039] Figure 1 A schematic diagram of a cross-view collaborative perception system for wide-area search according to an embodiment of the present invention;

[0040] Figure 2 This is a schematic diagram of the principle of point-line combined body pose estimation according to an embodiment of the present invention;

[0041] Figure 3 Schematic diagram of the distributed RANSAC point cloud matching principle of an embodiment of the present invention.

[0042] Figure 4The present invention is a schematic diagram of a cross-view multi-feature pose estimation process. DETAILED DESCRIPTION

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

[0044] The embodiment of the present invention provides a multi-feature pose estimation method for cross-viewing scenes, such as Figure 1 As shown in the figure, the subjects of collaborative positioning are drones and unmanned vehicles. They are both equipped with camera sensors to obtain image information in the environment. The entire system is divided into two parts: single-machine perception and collaborative perception.

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

[0046] For ease of description, this embodiment refers to drones and unmanned vehicles as intelligent entities.

[0047] Based on the above architecture, cross-view collaborative positioning is performed, including the following steps:

[0048] S1: During the single-agent positioning process, point features and line features are extracted from the image information in the binocular camera. Point features are composed of FAST corner points and MobileNetVLAD descriptors, while line features are extracted using LSD features, which are represented by the first and last points of the line segment and the LBD descriptor. Point features are uniformly extracted, and line features are length-filtered and extracted.

[0049] S2: The single agent integrates the point features and line feature information obtained in step S1 and extracts the point-line combination through the expansion algorithm. The specific steps of the expansion algorithm are to randomly select the endpoints of the line segment in a frame, search around it and find the nearest point, solve the angle between the direction of the current point at the other end of the line segment of the nearest point and the current straight line. If it is greater than the designed threshold, the two line segments are broken lines and are not merged; finally, solve the distance between the far point of the current line and the endpoints of the two ends of the nearest line. If the far distance is less than the short 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] Among them, 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 coordinate of the first point in the oth line segment, p p2 is the pixel coordinate of the second point in the pth line segment.

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

[0053] Specifically, considering that the point-line combination has strong geometric characteristics compared to other single features, whether across viewpoints or in normal odometer conditions, using this as the basis for pose estimation can reduce the calculation time while improving the accuracy of pose estimation.

[0054] S3: Based on step S2, if, ideally, there are related point-line combinations in two frames during the positioning process of the intelligent agent, the point-line combination is selected as the basis for initial pose estimation during the movement process. Figure 2 As shown in the figure, in environments with strong geometric features such as buildings and roads, such as cities and villages, there are certain point-line combinations. Most of these features are static and can be clearly distinguished from the surrounding environment. Therefore, in the process of pose estimation, constraints are added around the point-line combination. The specific expression is as follows

[0055]

[0056] in, is the Lie group representation of the camera posture, and the following The relevant parameters are all expressed in Lie groups, b represents the bth feature, k represents the total number of corresponding features, then b p Represented as the b-th point feature, k p Expressed as 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 projection of the point-line combination, K is the camera internal parameter matrix, which corresponds to the mapping of the three-dimensional coordinates of the object 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 and line feature points in the environment.

[0057] Furthermore, in the pose calculation process, the errors of point features and line features in the environment will be detected based on the calculation error of the point-line combination. Point and line features with large deviations will be eliminated as dynamic features to reduce the noise brought 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 transmitted during the agent interaction is the three-dimensional coordinates of the point-line combination, the related information of the line feature and the point feature, and the octree related node information.

[0059] S5: Based on the interactive process of step S4, the PCM algorithm is used through the point-line combination to achieve rough pose estimation and octree grid alignment.

[0060] Specifically, we optimize based on the angle information of multiple line segments of the point-line combination and the three-dimensional coordinates of the point as constraints. For consistency check, we use cosine distance for comparison. The specific formula is:

[0061]

[0062] Among them, i and j represent the i-th and j-th agents that interact, respectively, and e represents the calculation error. It is represented by the optimal relative pose estimation obtained by the PCM algorithm; n and m represent the number of aerial drones and ground unmanned vehicles; p and q represent the three-dimensional coordinate sets of the matching points in the maps of the two agents, respectively; It is represented as a set of line segment vectors; w and q respectively represent the w-th and q-th line feature vector information sets in the corresponding agent.

[0063] Furthermore, for the grid networks with the first and second largest similarity among the aligned octree grids.

[0064] S6: After completing the octree grid alignment in step S5, the distributed RANSAC algorithm is executed, such as Figure 3 As shown in the figure, the nxn grids of the octree are used as the basic operator, and the relevant matching information is extracted by extracting the parent node form, and the pose optimization operation is performed inside the basic operator. The specific formula is as follows:

[0065]

[0066] in, represents the best estimated pose calculated by the child node grid; p and q represent the three-dimensional coordinate sets of the matching points in the two agent maps; K represents the camera intrinsic parameter matrix, R represents the rotation in the pose estimation, t represents the translation matrix in the calculated 4x 4 transformation matrix, d and m represent the relevant parameters in the Plucker coordinate system, expressed as d=yx, m=x×y, x and y are the horizontal and vertical coordinates in the Plucker coordinate system respectively; β represents the weights for calculating point and line features respectively.

[0067] S7: The relative pose estimation calculated in each basic operator obtained in step S6 is extracted to form a relative pose estimation set, and RANSAC processing is further performed based on the set to achieve high-precision multi-agent relative pose estimation.

[0068] The relative pose estimation extracted from each grid is extracted to form a pose set, and RANSAC processing is further performed based on the set. The specific formula is as follows

[0069]

[0070] in, It represents the optimal pose estimate obtained by performing RANSAC calculation on the pose calculated by the parent node for the child node.

[0071] When multiple agents collaborate, the agents are unmanned vehicles or drones. When there is no interaction, each unmanned vehicle and / or drone executes steps S1-S3 respectively. When there is interaction, multiple drones and / or unmanned vehicles jointly execute steps S5-S7. Considering that the environmental information is screened twice during the matching process, and the huge map information is calculated in the form of distributed operators, it can not only improve the estimation accuracy, but also reduce the required computing cost.

[0072] Although the present invention focuses on the air-ground collaborative heterogeneous relative pose estimation method, the method still has certain reference significance for homogeneous multi-agent collaborative mapping. That is, complex features are constructed by combining simple features to achieve high-precision estimation, and there is a certain deviation between observations of different agents from different perspectives.

[0073] Those skilled in the art can easily understand that the above contents are only preferred embodiments of the present invention and cannot be used as limitations of the present invention. Any modification, equivalent replacement or improvement made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A multi-feature pose estimation method for cross-view scenes, characterized in that: The following steps are involved: S1: During the single-agent positioning process, point features and line features are extracted from the image information in the binocular camera; S2: The single agent integrates the point features and line feature information obtained in step S1 and extracts the point-line combination through the expansion algorithm; S3: Based on step S2, if there are related point-line combinations in two frames during the positioning process of the single agent, the single agent constructs a cost function for inter-frame pose estimation by combining point features, line features, and point-line combinations to perform inter-frame pose estimation; S4: Construct an octree map for the point-line related features in the environment obtained by the single agent in steps S1-S3, and transmit the three-dimensional coordinate information of the point-line combination, related information of line features and point features, and octree related node information during the multi-agent interaction; S5: Based on the interactive process of step S4, PCM algorithm is used to realize rough pose estimation and octree grid alignment through point-line combination; S6: After completing the octree grid alignment in step S5, execute the distributed RANSAC algorithm, use the octree nxn grids as the basic operator, extract the relevant matching information by extracting the parent node form, and perform pose optimization operations on the basic operator; S7: The relative pose estimation calculated in each basic operator obtained in step S6 is extracted to form a relative pose estimation set, and RANSAC processing is further performed based on the set to achieve high-precision multi-agent relative pose estimation.

2. A multi-feature pose estimation method for cross-view scenes as claimed in claim 1, characterized in that: The point features are composed of FAST corner points and MobileNetVLAD descriptors, and the point features are uniformly extracted in step S1; The line feature is extracted using LSD features, which are represented by the first and last points of the line segment and the LBD descriptor. In step S1, the line feature is length-screened and extracted.

3. The multi-feature pose estimation method for cross-view scenes according to claim 1, characterized in that: The specific steps of the expansion algorithm in step S2 are to randomly select the endpoint of a line segment in a frame, search around it and find the nearest point, solve the angle between the direction of the current point at the other end of the line segment of the nearest point and the current straight line. If it is greater than the designed threshold, the two line segments are broken lines and are not merged; finally, solve the distance between the far point of the current line and the endpoints of the two ends of the nearest line. If the far distance is smaller than the short 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 according to claim 3, characterized in that: The specific formula of the expansion algorithm in step S2 is: Among them, 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 coordinate of the first point in the oth line segment, p p2 is the pixel coordinate of the second point in the pth line segment.

5. The multi-feature pose estimation method for cross-view scenes according to claim 1, characterized in that: In step S3, external points in point features or line features are removed based on the point-line combination before estimation. The specific formula of the cost function is: in, is the Lie group representation of the camera posture, and the following The relevant parameters are all expressed in Lie groups, b represents the bth feature, k represents the total number of corresponding features, then b p Represented as the b-th point feature, k p Expressed as 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 projection of the point-line combination, K is the camera internal parameter matrix, which corresponds to the mapping of the three-dimensional coordinates of the object 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 and line feature points in the environment.

6. The multi-feature pose estimation method for cross-view scenes according to claim 1, characterized in that: Step S5 specifically includes the following steps: S51: In order to optimize based on the angle information of multiple line segments of the point-line combination and the three-dimensional coordinates of the point as constraint conditions, the cosine distance is used for comparison to check the consistency of the data; S52: Align the grid networks with the highest and second highest similarity among the octree grids.

7. A multi-feature pose estimation method for cross-viewing scenes as claimed in claim 6, characterized in that: The specific formula in step S51 is: Among them, i and j represent the i-th and j-th agents that interact, respectively, and e represents the calculation error. It is represented by the optimal relative pose estimation obtained by the PCM algorithm; n and m represent the number of aerial drones and ground unmanned vehicles; p and q represent the three-dimensional coordinate sets of the points that match each other in the maps of the two agents, respectively; It is represented as a set of line segment vectors; w and q respectively represent the w-th and q-th line feature vector information sets in the corresponding agent.

8. The multi-feature pose estimation method for cross-view scenes according to claim 1, characterized in that: The specific formula in step S6 is: in, represents the best estimated pose calculated by the child node grid; p and q represent the three-dimensional coordinate sets of the matching points in the two agent maps; K represents the camera intrinsic parameter matrix, R represents the rotation in the pose estimation, t represents the translation matrix in the calculated 4x4 transformation matrix, d and m are the relevant parameters in the Plucker coordinate system, expressed as d=yx, m=x×y, x and y are the horizontal and vertical coordinates in the Plucker coordinate system respectively; β represents the weights for calculating point and line features respectively.

9. The multi-feature pose estimation method for cross-view scenes according to claim 1, characterized in that: The specific formula of RANSAC in step S7 is: in, It represents the optimal pose estimate obtained by performing RANSAC calculation on the pose calculated by the parent node for the child node.

10. The multi-feature pose estimation method for cross-view scenes according to claim 1, characterized in that: When multiple intelligent agents collaborate, the intelligent agents are unmanned vehicles or drones. When no interaction occurs, each unmanned vehicle and / or drone executes steps S1-S3 separately. When interaction occurs, multiple drones and / or unmanned vehicles jointly execute steps S5-S7.

Images