Target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization

Abstract

The embodiments of the present application disclose a method and device for estimating the six-degree-of-freedom pose of a target based on binocular vision and particle swarm optimization, which relates to the field of computer vision. The method comprises: obtaining a binocular image of a target; the binocular image includes at least the rotation matrix, displacement vector, and geometric features of the target; decomposing the rotation matrix into a rotation vector and a rotation angle by a rotation matrix decomposition method, uniformly sampling the rotation vector and the rotation angle in six degrees of freedom to obtain a data point set; establishing a mathematical relationship between the rotation matrix and the displacement vector based on the geometric properties of the geometric features and the data point set; and performing target pose estimation on the binocular image of the target by designing an objective function and applying an optimization algorithm to obtain a pose estimation result. The present application solves the problems of low accuracy, poor versatility, and poor robustness existing in the related art.

Description

Technical Field

[0001] The present application relates to the field of computer vision, and in particular to a target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization. Background Art

[0002] Relative object pose estimation has numerous applications in aerospace and industrial assembly. In aerospace, estimating the relative pose between two spacecraft is a key issue in space rendezvous and docking. In planetary exploration, astronauts require highly reliable, autonomous, and accurate relative position and pose estimation algorithms to achieve landing. In industry, automated assembly requires estimating the pose of an object relative to a camera.

[0003] Depending on the selected image features, the pose estimation methods can be roughly divided into five categories: point-based, template-based, segmentation-based, edge-based, and basic geometric feature-based. Researchers have noticed that clear basic geometric features on the target object, such as 3-D straight line segments and 3-D circles, have good robustness.

[0004] However, existing pose estimation methods limit the number of basic geometric features and require geometric constraints between basic geometric features as prior knowledge, resulting in poor versatility of the methods. In addition, in real application scenarios, some basic geometric features are affected by lighting and occlusion and cannot be imaged in the image, resulting in poor robustness of the methods.

[0005] Therefore, there is an urgent need for a target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization with high accuracy, versatility and robustness. Summary of the Invention

[0006] The embodiments of the present application provide a method, device, electronic device and storage medium for estimating the six-degree-of-freedom pose of a target based on binocular vision and particle swarm optimization to solve the problems of low accuracy, poor versatility and robustness existing in related technologies.

[0007] In order to solve the above technical problems, the technical solutions adopted in this application are:

[0008] According to one aspect of the present application, a method for estimating the six-degree-of-freedom pose of a target based on binocular vision and particle swarm optimization, the method comprising obtaining a binocular image of the target; the binocular image at least comprising the rotation matrix, displacement vector and geometric features of the target; decomposing the rotation matrix into a rotation vector and a rotation angle by a rotation matrix decomposition method, performing six-degree-of-freedom uniform sampling on the rotation vector and the rotation angle to obtain a data point set; establishing a mathematical relationship between the rotation matrix and the displacement vector based on the geometric properties of the geometric features and the data point set; performing target pose estimation on the binocular image of the target by designing an objective function and applying an optimization algorithm to obtain a pose estimation result; the objective function is used to estimate the pose of the target; the optimization algorithm is used to optimize the pose estimation result; the pose estimation result is used to display the six-degree-of-freedom pose of the target and determine the position and orientation of the target.

[0009] In an exemplary embodiment, decomposing the rotation matrix into a rotation vector and a rotation angle by using a rotation matrix decomposition method is achieved by the following steps: decomposing the rotation matrix into a rotation vector and a rotation angle by using the Rodrigues formula.

[0010] In an exemplary embodiment, the rotation vector and the rotation angle are uniformly sampled in six degrees of freedom to obtain a data point set by the following steps: the rotation vector is uniformly sampled in six degrees of freedom on the unit sphere by the Thomson method; the rotation angle between 0 and π is uniformly sampled to obtain a data point set, ensuring that the data point set includes the posture changes of the target in six degrees of freedom; the six degrees of freedom include three translational degrees of freedom and three rotational degrees of freedom.

[0011] In an exemplary embodiment, establishing a mathematical relationship between the rotation matrix and the displacement vector based on the geometric properties of the geometric features and the data point set is achieved by: calculating a geometric distance from a projection point of the target on the binocular image to the data point set using a sampled Sampson distance; the geometric distance is a geometric property of the geometric features; the geometric features include points, lines, and surfaces; and establishing a mathematical relationship between the rotation matrix and the displacement vector based on the geometric distance from the projection point to the data point set.

[0012] In an exemplary embodiment, the optimization algorithm includes a particle swarm optimization algorithm; by designing an objective function and applying the optimization algorithm, the target pose is estimated for the binocular image of the target, and the pose estimation result is obtained by the following steps: designing an objective function based on the geometric relationship and rotation posture of the binocular image in stereo vision; the objective function contains constraints on the geometric relationship and rotation posture in stereo vision; using the particle swarm optimization algorithm to find the optimal rotation matrix and displacement vector in the parameter space between the rotation matrix and the displacement vector, and minimizing the objective function based on the found optimal rotation matrix and displacement vector; based on the objective function minimized by the particle swarm optimization algorithm, estimating the relative pose of the target with respect to the target coordinate system, and obtaining the pose estimation result; the target coordinate system is used to quantify the pose change of the target.

[0013] In an exemplary embodiment, the objective function is designed by parameterizing the rotation matrix of the target to obtain the geometric distance of the straight line and circle geometric features of the target in the binocular image; obtaining the corresponding Sampson distance based on the geometric distance, and constructing the objective function based on the Sampson distance.

[0014] In an exemplary embodiment, after obtaining the pose estimation result, the method further includes the following steps: optimizing the relative pose of the target in the pose estimation result by using the Levenberg-Marquardt algorithm.

[0015] According to one aspect of the present application, a target six-degree-of-freedom pose estimation device based on binocular vision and particle swarm optimization, the device including a data acquisition module for acquiring a binocular image of the target; the binocular image includes at least the rotation matrix, displacement vector and geometric features of the target; a uniform sampling module for decomposing the rotation matrix into a rotation vector and a rotation angle by a rotation matrix decomposition method, and performing six-degree-of-freedom uniform sampling on the rotation vector and the rotation angle to obtain a data point set; a mathematical relationship construction module for establishing a mathematical relationship between the rotation matrix and the displacement vector according to the geometric properties of the geometric features and the data point set; a pose estimation module for performing target pose estimation on the binocular image of the target by designing an objective function and applying an optimization algorithm to obtain a pose estimation result.

[0016] According to one aspect of the present application, an electronic device includes at least one processor and at least one memory, wherein the memory stores program instructions or codes; the program instructions or codes are loaded and executed by the processor, so that the electronic device implements the target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization as described above.

[0017] According to one aspect of the present application, a storage medium stores program instructions or codes thereon, which are loaded and executed by a processor to implement the target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization as described above.

[0018] According to one aspect of the present application, a computer program product includes program instructions or codes, which are stored in a storage medium. The processor of an electronic device reads the program instructions or codes from the storage medium, loads and executes the program instructions or codes, so that the electronic device implements the target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization as described above.

[0019] The beneficial effects of the technical solution provided by this application are:

[0020] In the above technical solution, the present application solves the problems of low accuracy, poor versatility and robustness in the related technology.

[0021] Specifically, the present application first obtains a binocular image of the target, which includes at least the rotation matrix, displacement vector and geometric features of the target. The rotation matrix is ​​decomposed into a rotation vector and a rotation angle through a rotation matrix decomposition method. The rotation vector and the rotation angle are then uniformly sampled with six degrees of freedom to obtain a data point set to ensure that all-round posture changes are covered. Then, based on the geometric properties of the geometric features and the data point set, a mathematical relationship between the rotation matrix and the displacement vector is established. Finally, the target pose is estimated for the binocular image of the target by designing an objective function and applying an optimization algorithm to obtain a pose estimation result. Through the particle swarm optimization algorithm and the nonlinear optimization algorithm, the three basic geometric features of points, lines and circles can be processed simultaneously, and there is no limit on the number of features and the relationship between their positions. It has a very high accuracy and robustness, thereby solving the problems of low accuracy, poor versatility and robustness in the prior art.

[0022] It should be understood that the general description and the following detailed description are exemplary and explanatory only and are not restrictive of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present application and, together with the description, serve to explain the principles of the present application.

[0024] Figure 1 It is a schematic diagram of the implementation environment involved in this application;

[0025] Figure 2 This is a flow chart of a method for estimating a target six-degree-of-freedom pose based on binocular vision and particle swarm optimization according to an exemplary embodiment;

[0026] Figure 3 yes Figure 2 A flowchart of an embodiment corresponding to step 270 in an embodiment;

[0027] Figure 4 is a schematic diagram of projection of a point on a target coordinate system in stereo vision in an exemplary embodiment;

[0028] Figure 5 is a schematic diagram of a design objective function in an exemplary embodiment;

[0029] Figure 6 is a schematic diagram of constructing a mathematical relationship between a rotation matrix and a displacement vector in an exemplary embodiment;

[0030] Figure 7 is a schematic diagram of processing a circle feature in an exemplary embodiment;

[0031] Figure 8 is a schematic diagram of target pose optimization in an exemplary embodiment;

[0032] Figure 9 is a line graph showing comparative test results of an exemplary embodiment and a prior art;

[0033] Figure 10 is a line graph showing comparative test results of an exemplary embodiment and another prior art;

[0034] Figure 11 is a line graph of experimental results of the number of straight line features and circle features of an exemplary embodiment;

[0035] Figure 12 This is a schematic diagram of the implementation results of a target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization in Model 1, Model 2, and Model 3 in an application scenario;

[0036] Figure 13 This is a block diagram of a target six-degree-of-freedom pose estimation device based on binocular vision and particle swarm optimization according to an exemplary embodiment;

[0037] Figure 14 is a schematic structural diagram of a server according to an exemplary embodiment;

[0038] Figure 15 It is a block diagram of an electronic device according to an exemplary embodiment. DETAILED DESCRIPTION

[0039] The following describes in detail embodiments of the present application, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present application, and are not to be construed as limiting the present application.

[0040] It will be understood by those skilled in the art that, unless expressly stated otherwise, the singular forms "a", "an", "said" and "the" used herein may also include the plural forms. It should be further understood that the term "comprising" used in the specification of the present application refers to the presence of the features, integers, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or groups thereof. It should be understood that when we refer to an element as being "connected" or "coupled" to another element, it may be directly connected or coupled to the other element, or there may be intermediate elements. In addition, "connected" or "coupled" as used herein may include wireless connections or wireless couplings. The term "and / or" used herein includes all or any units and all combinations of one or more associated listed items.

[0041] The following is an introduction and explanation of several terms involved in this application:

[0042] Binocular Vision: Binocular vision refers to a visual method in which two eyes simultaneously observe the same scene to obtain depth and three-dimensionality. The image captured by each eye has a slightly different perspective. This difference is called disparity, which is interpreted by the brain as depth information of objects in the scene. Binocular vision is a common depth perception mechanism in humans and many other vertebrates.

[0043] Particle Swarm Optimization (PSO) is a meta-heuristic algorithm inspired by the behavior of groups such as bird flocks or fish schools. The algorithm was originally proposed by Russell C. Eberhart and James Kennedy in 1995. The basic idea of ​​PSO is to search for the optimal solution by simulating the cooperation and competition between individuals in the group. In PSO, the solution to the problem is called a particle, which searches for the optimal solution in the solution space of the problem. Each particle has a position and a velocity. It adjusts its movement direction and speed according to its own historical optimal position and the historical optimal position of the group, as well as its current position and velocity. In this way, the entire particle swarm finds the optimal solution in the solution space by continuously adjusting its position and velocity. The performance of PSO is affected by parameter settings, including the number of particles, inertia weight, acceleration factor, etc. Appropriate parameter settings can make the algorithm better converge to the global optimal solution. PSO is widely used in function optimization, neural network training, image processing and other fields.

[0044] Target six-degree-of-freedom pose estimation: refers to the six-degree-of-freedom estimation of the position and posture (pose) of an object or target in space. In three-dimensional space, the six-degree-of-freedom pose of an object includes three translational degrees of freedom (translation along the x, y, and z axes) and three rotational degrees of freedom (rotation around the x, y, and z axes). Therefore, the goal of target six-degree-of-freedom pose estimation is to determine the position and orientation of the object. In the fields of computer vision, robotics, virtual reality, etc., six-degree-of-freedom pose estimation is a key issue that involves obtaining the accurate position and orientation of the target in space from sensor data or images.

[0045] Geometric features: refers to the measurable attributes that geometrically describe the shape and structure of an object or image, providing geometric information about the object or scene, such as area, perimeter, center position diameter, etc.

[0046] Pose estimation and tracking algorithms based on monocular cameras and basic geometric features have been proven feasible and have been widely studied. The difference between binocular cameras and monocular cameras is that in order to obtain unambiguous pose estimates, the pose estimation algorithm based on monocular cameras requires more geometric features. If the geometric features on the target are insufficient, it is necessary to take two or more images of the target from different angles. In contrast, the pose estimation algorithm based on binocular cameras obtains image information of the basic geometric features of the target from stereo image pairs, so it requires fewer geometric features and has better robustness.

[0047] Hu proposed a stereo vision pose estimation algorithm based on a circle and two straight lines. This method reconstructs the spatial circle and spatial straight line segments through a stereo camera, and estimates the pose by the direction of the spatial straight line segments and the direction of the rotation axis of the spatial circle; Jian proposed a stereo vision pose estimation algorithm based on a circle and four feature points. This method reconstructs the spatial circle through a stereo camera, and combines the four feature points coplanar with the circle to reconstruct the rotation matrix, and finally uses the orthogonal projection iterative optimization algorithm to estimate the pose of the target; the disadvantages of the above two methods are that the number of basic geometric features is limited, and some positional relationships are required between the basic geometric features, such as the straight lines must be parallel to each other, etc., both have restrictions and are difficult to generalize.

[0048] From the above, we can see that the relevant technologies still have defects such as low accuracy, poor versatility and robustness.

[0049] To this end, the target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization provided in the present application can effectively improve the accuracy, versatility and robustness of the target six-degree-of-freedom pose estimation based on binocular vision and particle swarm optimization. Accordingly, the target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization is suitable for a target six-degree-of-freedom pose estimation device based on binocular vision and particle swarm optimization. The target six-degree-of-freedom pose estimation device based on binocular vision and particle swarm optimization can be deployed in an electronic device configured with a von Neumann architecture. For example, the electronic device can be a desktop computer, a laptop computer, a server, etc.

[0050] In order to make the objectives, technical solutions and advantages of this application clearer, the implementation methods of this application will be further described in detail below with reference to the accompanying drawings.

[0051] Figure 1 The schematic diagram of the implementation environment involved in a target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization is shown in FIG. The implementation environment includes a collection end 110 and a server end 130 .

[0052] Specifically, the acquisition end 110 acquires a binocular image of the target. The acquisition end 110 may be any electronic device having a binocular image acquisition function and is not limited here.

[0053] The acquisition terminal 110 and the server terminal 130 may be connected via a wired or wireless communication connection to achieve data transmission between the two. For example, the transmitted data may be a binocular image.

[0054] The server end 130 can also be considered as a cloud, cloud platform, platform end, service end, etc. The server end 130 can be a single server, a server cluster composed of multiple servers, or a cloud computing center composed of multiple servers, so as to better provide backend services to the massive acquisition end 110. For example, the backend service includes a target six-degree-of-freedom pose estimation service based on binocular vision and particle swarm optimization.

[0055] With the interaction between the acquisition end 110 and the server end 130, in an application scenario, taking the server end 130 providing a target six-degree-of-freedom pose estimation service based on binocular vision and particle swarm optimization as an example, after the acquisition end 110 obtains the binocular image of the target, the rotation matrix, displacement vector and geometric features of the target included in the binocular image are sent to the server end 130. Then, the server end 130 can receive the rotation matrix, displacement vector and geometric features of the target sent by the acquisition end 110, and then provide the target six-degree-of-freedom pose estimation service based on binocular vision and particle swarm optimization based on the rotation matrix, displacement vector and geometric features of the target. Specifically, after the server end 130 obtains the rotation matrix, displacement vector and geometric features of the target, it decomposes the rotation matrix into a rotation vector and a rotation angle through the rotation matrix decomposition method, and then uniformly samples the rotation vector and the rotation angle in six degrees of freedom to obtain a data point set to ensure that all-round posture changes are covered. Then, based on the geometric properties of the geometric features and the data point set, a mathematical relationship between the rotation matrix and the displacement vector is established. Finally, by designing an objective function and applying an optimization algorithm, the target binocular image is used to estimate the target pose and obtain the pose estimation result.

[0056] See also Figure 2 The embodiment of the present application provides a target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization. The method is applicable to electronic devices, which can be Figure 1 The server end 130 in the illustrated implementation environment may also be a desktop computer, a laptop computer, a server, etc.

[0057] In the following method embodiments, for ease of description, the execution subject of each step of the method is taken as an electronic device as an example for illustration, but this does not constitute a specific limitation.

[0058] like Figure 2 As shown, the method may include the following steps:

[0059] Step 210: Acquire a binocular image of the target.

[0060] Among them, binocular images are images of the same scene or object captured simultaneously by two cameras (left camera and right camera). This camera arrangement simulates the human binocular vision system and can provide depth information. The image captured by each camera is called a binocular stereo image pair, where one image corresponds to the left eye perspective and the other image corresponds to the right eye perspective. The difference between the two images, also called disparity, is very important information in depth perception. By calculating the disparity, the depth or distance of different points in the scene can be obtained.

[0061] In a possible implementation, the binocular image includes at least a rotation matrix, a displacement vector, and geometric features of the target.

[0062] In one possible implementation, a binocular image of a target may be obtained by first selecting and installing a binocular camera, then calibrating the binocular camera to ensure that the parameters and positions between the two cameras are accurate, and then using corresponding software or programming languages ​​(such as the OpenCV library in Python) to capture the image of the binocular camera, ensuring that the images of the two cameras are captured synchronously for subsequent depth calculation, using a stereo matching algorithm to calculate the depth information, and generating a depth map based on the results of the stereo matching.

[0063] Step 230 : Decompose the rotation matrix into a rotation vector and a rotation angle by using a rotation matrix decomposition method, perform six-degree-of-freedom uniform sampling on the rotation vector and the rotation angle, and obtain a data point set.

[0064] In a possible implementation, the rotation matrix is ​​decomposed into a rotation vector and a rotation angle using the Rodriguez formula.

[0065] The specific calculation formula is as follows:

[0066]

[0067] Among them, the symbol in the upper left corner of the parameter represents the coordinate name, for example, l, r, and o represent the left camera coordinate system, the right camera coordinate system, and the target coordinate system respectively; ordinary font represents scalars; bold font represents matrices or vectors; R and t represent rotation matrices and displacement vectors respectively; I represents the three-dimensional unit matrix.

[0068] Specifically, the calculation formula of the rotation vector n is as follows:

[0069] n=[n1 n2 n3] T

[0070] =[cosαsinβ sinαsinβ cosβ] T ,0≤α<2π,0≤β<π

[0071]

[0072] In one possible implementation, the Thomson method is used to uniformly sample the six degrees of freedom of the rotation vector on the unit sphere, and the rotation angles between 0 and π are uniformly sampled to obtain a data point set, ensuring that the data point set includes the posture changes of the target in the six degrees of freedom, where the six degrees of freedom include three translational degrees of freedom and three rotational degrees of freedom.

[0073] In this way, the embodiment of the present application uses the Thomson method to uniformly sample the rotating vector on the unit sphere. The Thomson method aims to minimize the electric potential energy of the system. The sampled rotating vector is more uniform on the sphere, avoiding the deviation that may be introduced by other non-uniform sampling methods, and achieving a more stable and orderly point distribution. The physical principle behind the Thomson method is based on Coulomb's law, which simulates the charge interaction between points. This method based on physical principles is easier to understand and explain, and can also be highly adjustable by adjusting the constants in Coulomb's law and the initial configuration optimization process.

[0074] In a possible implementation, after the motion representation of the target is achieved through the above process, a stereo camera model is constructed for the target based on the left camera and the right camera.

[0075] Specifically, if Figure 4 As shown, with the optical center of the left camera and the right camera as the origin and the optical axis as the axis, establish l O- l X l Y l Z and r O- r X r Y r Z coordinate system, the image plane is l Z=f l and r Z=f r Where f l and f r Represent the focal lengths of the left and right cameras respectively, and the image plane coordinate system is set to o imgl - l u l v and o imgr - r u r v, where l u、 l v. r u、 r v respectively l X、 l Y. r X、 r Y parallel.

[0076] The horizontal line above the vector symbol indicates the projection of the basic geometric features represented by the vector in the image, for example: express o The projection of p on the left image, given the intrinsic parameter matrices and conditions of the left and right cameras, and and o The specific relationship of p is as follows:

[0077]

[0078]

[0079] in, l K 12 and r K 12 Respectively l K and r The matrix formed by the first two rows of K, l K3 and r K3 represents l K and r The matrix formed by the third row of K; l R o and l t o Represents the rotation matrix and displacement vector of the target coordinate system relative to the left camera coordinate system, r R o and r t o Represents the rotation matrix and displacement vector of the target coordinate system relative to the left camera coordinate system. Given the rotation matrix of the two cameras r R l and displacement vector r t l Under the conditions, r R o and r t o The specific representation is as follows:

[0080] r R o = r R l l R o ,

[0081] r t o = r R l l t o + r t l .

[0082] Through the above process, the embodiment of the present application constructs a stereo camera model of the target. By simulating the human binocular vision system, the depth information of the target in the scene is obtained, and the distance of the target from the camera can be inferred, providing more accurate object positioning information. It can make up for the defect that a monocular camera may not be able to accurately estimate the distance of an object in the absence of depth information, improve the accuracy of depth perception, and be more robust to environments with changing lighting and lack of texture.

[0083] Step 250 : Establishing a mathematical relationship between the rotation matrix and the displacement vector based on the geometric properties of the geometric features and the data point set.

[0084] Specifically, step 250 may include the following steps:

[0085] Step 251 calculates the geometric distance between the projection point of the target on the binocular image and the data point set by sampling the Sampson distance.

[0086] The geometric distance is a geometric property of a geometric feature, and the geometric features include points, lines, and surfaces.

[0087] Step 253: Establish a mathematical relationship between the rotation matrix and the displacement vector based on the geometric distance from the projection point to the data point set.

[0088] Step 270 , by designing an objective function and applying an optimization algorithm, the target pose is estimated for the binocular image of the target to obtain a pose estimation result.

[0089] Among them, the objective function is used to estimate the pose of the target, the optimization algorithm is used to optimize the pose estimation results, and the pose estimation results are used to display the six-degree-of-freedom pose of the target and determine the position and orientation of the target.

[0090] In one possible implementation, the optimization algorithm includes a particle swarm optimization algorithm.

[0091] Specifically, if Figure 3 As shown, step 270 may include the following steps:

[0092] Step 310 : Design an objective function based on the geometric relationship and rotational posture of the binocular images in stereo vision.

[0093] In one possible implementation, the objective function is designed by first parameterizing the rotation matrix of the target, obtaining the geometric distance of the target's straight line and circle geometric features in the binocular image, then obtaining the corresponding Sampson distance based on the geometric distance, and finally constructing the objective function based on the Sampson distance.

[0094] Specifically, the rotation matrix R is parameterized and recorded as Ω = [α β θ]T T , the design objective function is as follows:

[0095]

[0096] Among them, d i Represents the geometric distance from the data point to the line segment, s i Represents the Sampson distance from the data point to the ellipse, N L and N L Respectively represent the number of data points for the straight line and the ellipse on the image; d i is calculated as follows:

[0097]

[0098] Among them, q i represents the i-th straight line data point, Represents the distance q on the line segment i The nearest point, The calculation method is as follows:

[0099]

[0100] λ=Median(0 λ * 1),

[0101]

[0102] in, and They represent the image of the model line on the image, and Median() means taking the median, such as Figure 5 and Figure 6 As shown, the introduction of λ and λ * This is to avoid the data points falling on the straight line where the image of the straight line segment of the model is located, because accurate pose estimation should make the data points fall on the image of the straight line segment of the model.

[0103] In one possible implementation, the s in the objective function i (Ω) represents the Sampson distance from the data point to the ellipse, and the specific calculation formula is as follows:

[0104]

[0105] in, The data point q representing the circle in the image i The homogeneous coordinate form of ; and Respectively The first and second features; C′ represents the matrix form of the image of the space circle calculated according to the posture. The specific calculation formula is as follows:

[0106] C′=κK -T((δI-ηC T )(δI-Cη T )-r 2 ηη T )K -1 ,

[0107] Wherein, κ represents a non-zero constant; K represents the camera intrinsic parameter matrix (in this embodiment, it represents l K or r K); δ represents the distance from the optical center to the supporting plane of the circle; η represents the unit normal vector of the supporting plane of the space circle; C represents the coordinates of the space circle in the camera coordinate system; r represents the radius of the space circle.

[0108] Through the above process, the embodiment of the present application obtains an objective function that is conducive to obtaining the accurate position of the target. i (Ω) represents the distance from the data point to the image of the spatial straight line segment in the image, which ensures that the image of the model is surrounded by the data points as much as possible. i (Ω) represents the Sampson distance, which is more efficient and accurate than directly solving the geometric distance.

[0109] Step 330 : searching for the optimal rotation matrix and displacement vector in the parameter space between the rotation matrix and the displacement vector by using a particle swarm optimization algorithm, and minimizing the objective function based on the found optimal rotation matrix and displacement vector.

[0110] Step 350 : Estimate the relative position of the target with respect to the target coordinate system according to the objective function minimized by the particle swarm optimization algorithm.

[0111] Among them, the target coordinate system is used to quantify the pose change of the target.

[0112] Specifically, the Particle Swarm Optimization (PSO) algorithm abstracts the solution to each optimization problem into a massless and volumeless particle in the search space. All particles have a fitness value determined by the objective function and a speed that determines the direction and distance of the particle's movement. The particles know their best position found so far and their current position. In addition, each particle also knows the best position found so far by all particles in the entire swarm. The particle determines its own movement speed based on its own best position and the best position found by the swarm, ultimately finding the global optimal solution.

[0113] In this embodiment of the application, the rotation matrix R and the translation vector t are estimated by PSO. The degree of freedom of the rotation matrix and the degree of freedom of the translation vector determine the dimension d=6 of the particle swarm search space. Now assume that the position and velocity of the i-th particle in the d-dimensional search space are X i =[x i1x i2 ... x id ] and V i =[v i1 v i2 ... v id ], the initial values ​​of the position X and velocity V of all particles are randomly generated. In each iteration, the particle updates itself by tracking two optimal solutions, one of which is the optimal solution found by the particle itself, denoted as Pbest i =[pbest i1 pbest i2 ... pbest id ] T The second is the optimal solution found by the entire group, denoted as Gbest g =[gbest1 gbest2 ... gbest d ] T , the particle updates its velocity and position according to the following formula:

[0114]

[0115]

[0116] Where j = 1, 2, ..., d, k represents the current iteration number, w represents the inertia weight, c1 and c2 represent positive learning factors, and r1 and r2 are uniformly distributed random numbers between 0 and 1.

[0117] The physical meaning of the functional relationship of t with respect to R constructed in the embodiment of the present application is: projecting the CAD model onto the image according to the posture (R and t), and making the geometric features as close as possible to the corresponding data points on the image; in terms of straight line features, the projection of the straight line features according to the posture should fall as much as possible within the corresponding data point (pixel point) set, such as Figure 6 As shown in , if the geometric distance from the projection point to the fitting line of the data point set is used as the functional relationship, it is possible that the projection point falls outside the data point set area but is still on the fitting line of the point set. Therefore, Figure 5 As shown in , taking the distance from the projection point to the fitting straight line segment of the data point set as the functional relationship can ensure that the projection point falls within the data point set area as much as possible; in terms of circle features, such as Figure 7 As shown, the embodiment of the present application samples the Sampson distance because directly calculating the geometric distance from the projection point to the fitting curve of the data point set requires a lot of time and will greatly reduce the efficiency, while the Sampson distance can approximate the geometric distance and is easy to calculate.

[0118] In one possible implementation, after obtaining the pose estimation result according to the objective function minimized by the particle swarm optimization algorithm, the relative pose of the target in the pose estimation result is optimized by the Levenberg-Marquardt algorithm.

[0119] Specifically, in the Levenberg-Marquardt algorithm optimization process, the update formula in the kth iteration is as follows:

[0120]

[0121] Among them, such as Figure 8 As shown, in each iteration, it is necessary to calculate the image of the geometric feature closest to the data point in the image. The closest point of the straight line can be directly calculated. However, since calculating the image of the spatial circle closest to the data point in the image requires a lot of calculations, the embodiment of the present application introduces an intermediate variable The nearest point is replaced by a close point to avoid calculating the point closest to the data point on the spatial circle in each iteration. Through the update formula designed in the embodiment of the present application, the corresponding pose (R and t) when the objective function converges is the final target pose estimation result.

[0122] Through the above process, the embodiment of the present application first obtains a binocular image of the target, which includes at least the rotation matrix, displacement vector and geometric features of the target. The rotation matrix is ​​decomposed into a rotation vector and a rotation angle through a rotation matrix decomposition method, and then the rotation vector and the rotation angle are uniformly sampled with six degrees of freedom to obtain a data point set to ensure that all-round posture changes are covered. Then, based on the geometric properties of the geometric features and the data point set, a mathematical relationship between the rotation matrix and the displacement vector is established. Finally, by designing an objective function and applying an optimization algorithm, the target pose is estimated for the binocular image of the target to obtain a pose estimation result. Through the particle swarm optimization algorithm and the nonlinear optimization algorithm, the three basic geometric features of points, lines and circles can be processed simultaneously, and there is no limit on the number of features and the relationship between their positions. It has a very high accuracy and robustness, thereby solving the problems of low accuracy, poor versatility and robustness in the prior art.

[0123] Figure 9 A line graph showing comparative test results of an exemplary embodiment and a prior art is shown.

[0124] Specifically, if Figure 9-1 As shown, a comparative experiment of distance test was conducted between the embodiment of the present application and an existing technology. The experimental results show that the embodiment of the present application (pso line and pso+lm line) is significantly better, more stable and more robust than the existing technology in terms of errors in rotation matrix and displacement vector.

[0125] like Figure 9-2As shown, a comparative experiment of noise test was conducted on the embodiment of the present application and an existing technology. The experimental results show that the embodiment of the present application (pso line and pso+lm line) is significantly superior to the existing technology in rotation matrix estimation, and in displacement vector estimation, after the noise increases to 7, the error is smaller than that of the existing technology and the robustness is significantly enhanced.

[0126] like Figure 9-3 As shown, a comparative experiment of occlusion rate test was conducted between the embodiment of the present application and an existing technology. The experimental results show that the embodiment of the present application (pso line and pso+lm line) is obviously more accurate, more stable and more robust than the existing technology in terms of rotation matrix, and in terms of displacement vector, when the occlusion rate is less than 0.2, the estimation error is slightly greater than that of the existing technology, and when the occlusion rate is greater than 0.2, the estimation error is significantly better than that of the existing technology.

[0127] Figure 10 A line graph showing comparative test results of an exemplary embodiment and another prior art is shown.

[0128] Specifically, if Figure 10-1 As shown, a comparative experiment of distance test was conducted between the embodiment of the present application and an existing technology. The experimental results show that the embodiment of the present application (pso line and pso+lm line) has similar displacement vector estimation errors, but is obviously more accurate, stable and robust than the existing technology in rotation matrix estimation.

[0129] like Figure 10-2 As shown, a comparative experiment of noise test was conducted on the embodiment of the present application and an existing technology. The experimental results show that when the noise is less than 8 in terms of displacement vector, the error of the embodiment of the present application (pso line and pso+lm line) is slightly greater than that of the existing technology, and when the noise is greater than 8, the error is significantly better than that of the existing technology; and in terms of rotation matrix estimation, it is significantly more accurate, more stable and more robust than the existing technology.

[0130] like Figure 10-3 As shown, a comparative experiment of occlusion rate test was conducted on the embodiment of the present application and an existing technology. The experimental results show that in terms of displacement vector estimation of the embodiment of the present application (pso line and pso+lm line), when the occlusion rate is small (occlusion rate is less than 0.3), the error is slightly greater than that of the existing technology, but when the occlusion rate is large (occlusion rate is greater than 0.3), the error is significantly better than that of the existing technology; and in terms of rotation matrix estimation, the error is significantly smaller, more stable and more robust than that of the existing technology.

[0131] Figure 11 A line graph showing experimental results of the number of straight line features and circle features of an exemplary embodiment is shown.

[0132] Specifically, if Figure 11-1As shown, the embodiment of the present application conducted an experiment on the number of straight line features. It can be seen that as the number of straight line features increases, the errors of the rotation matrix and the displacement vector decrease.

[0133] like Figure 11-2 As shown, the embodiment of the present application conducted an experiment on the number of circular features. It can be seen that as the number of circular features increases, the errors of the rotation matrix and the displacement vector decrease.

[0134] Through the above process, the embodiment of the present application improves the accuracy of the rotation matrix and displacement vector, improves the accuracy of the displacement vector estimation, can simultaneously process three basic geometric features of points, lines, and circles, and there is no limit on the number of features and the relationship between their positions. It has a very high accuracy and robustness, thereby solving the problems of low accuracy, poor versatility and robustness in the existing technology.

[0135] Figure 12 This is a schematic diagram of the implementation results of a target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization in Model 1, Model 2, and Model 3 in an application scenario.

[0136] Specifically, if Figure 12-1 As shown, the embodiment of the present application conducted a physical experiment on model 1. Figure (a) is the left camera image, and Figure (b) is the right camera image. The circular lines are the images of the model 1 projected into each image according to the posture estimated by the embodiment of the present application; Figure 12-2 As shown, the embodiment of the present application conducted a physical experiment on model 2. Figure (a) is the left camera image, and Figure (b) is the right camera image. The circular lines are the images of model 2 projected into each image according to the posture estimated by the embodiment of the present application; Figure 12-3 As shown, the embodiment of the present application conducted a physical experiment on model 3. Figure (a) is the left camera image, and Figure (b) is the right camera image. The circular lines are the images of model 3 projected into each image according to the posture estimated by the embodiment of the present application. It can be seen from each figure that the image of the model is basically consistent with the real object, indicating that the posture estimated by the method provided by the present application is very close to the real posture.

[0137] The following is an embodiment of the device of the present application, which can be used to perform the target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization involved in this application. For details not disclosed in the embodiment of the device of the present application, please refer to the method embodiment of the target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization involved in this application.

[0138] See also Figure 13In an embodiment of the present application, a target six-degree-of-freedom pose estimation device 800 based on binocular vision and particle swarm optimization is provided, including but not limited to: a data acquisition module 810, a uniform sampling module 830, a mathematical relationship construction module 850 and a pose estimation module 870.

[0139] The data acquisition module 810 is used to acquire a binocular image of the target; the binocular image at least includes the rotation matrix, displacement vector and geometric features of the target.

[0140] The uniform sampling module 830 is used to decompose the rotation matrix into a rotation vector and a rotation angle by using a rotation matrix decomposition method, and perform uniform sampling of the rotation vector and the rotation angle in six degrees of freedom to obtain a data point set.

[0141] The mathematical relationship building module 850 is used to establish a mathematical relationship between the rotation matrix and the displacement vector according to the geometric properties of the geometric features and the data point set.

[0142] The pose estimation module 870 is used to estimate the target pose of the binocular image of the target by designing an objective function and applying an optimization algorithm to obtain a pose estimation result.

[0143] It should be noted that the target six-degree-of-freedom pose estimation device based on binocular vision and particle swarm optimization provided in the above embodiment only uses the division of the above-mentioned functional modules as an example when performing target six-degree-of-freedom pose estimation based on binocular vision and particle swarm optimization. In actual applications, the above-mentioned functions can be assigned to different functional modules as needed, that is, the internal structure of the target six-degree-of-freedom pose estimation device based on binocular vision and particle swarm optimization will be divided into different functional modules to complete all or part of the functions described above.

[0144] In addition, the target six-degree-of-freedom pose estimation device based on binocular vision and particle swarm optimization provided in the above embodiment and the target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization belong to the same concept, and the specific manner in which each module performs the operation has been described in detail in the method embodiment and will not be repeated here.

[0145] Figure 14 According to an exemplary embodiment, a server structure diagram is shown. The server is suitable for Figure 1 The server side 130 in an implementation environment is shown.

[0146] It should be noted that the server is only an example adapted for this application and cannot be considered to provide any limitation on the scope of use of this application. The server cannot be interpreted as needing to rely on or must have Figure 14 One or more components of exemplary server 2000 are shown.

[0147] The hardware structure of the server 2000 may vary greatly due to different configurations or performances, such as Figure 14 As shown, the server 2000 includes a power supply 210 , an interface 230 , at least one memory 250 , and at least one central processing unit (CPU) 270 .

[0148] Specifically, the power supply 210 is used to provide operating voltage for each hardware device on the server 2000 .

[0149] The interface 230 includes at least one wired or wireless network interface for interacting with external devices. Figure 1 The interaction between the terminal 100 and the server 200 in the implementation environment is shown.

[0150] Of course, in other examples adapted by this application, the interface 230 may further include at least one serial-to-parallel conversion interface 233, at least one input-output interface 235, and at least one USB interface 237, etc. Figure 14 As shown, this does not constitute a specific limitation.

[0151] The memory 250 serves as a carrier for resource storage and can be a read-only memory, random access memory, disk or optical disk, etc. The resources stored thereon include an operating system 251, application 253 and data 255, etc. The storage method can be temporary storage or permanent storage.

[0152] Among them, the operating system 251 is used to manage and control the hardware devices and application programs 253 on the server 200 to enable the central processing unit 270 to calculate and process the massive data 255 in the memory 250. It can be WindowsServerTM, Mac OS XTM, UnixTM, LinuxTM, FreeBSDTM, etc.

[0153] The application program 253 is a computer program that performs at least one specific task based on the operating system 251 and may include at least one module ( Figure 14 (not shown), each module can respectively include a computer program for the server 2000. For example, a target six-degree-of-freedom pose estimation device based on binocular vision and particle swarm optimization can be regarded as an application 253 deployed on the server 2000.

[0154] The data 255 may be a binocular image stored in a binocular camera, etc., and stored in the memory 250 .

[0155] The central processing unit 270 may include one or more processors and is configured to communicate with the memory 250 via at least one communication bus to read the computer program stored in the memory 250, thereby performing operations and processing on the massive data 255 in the memory 250. For example, the central processing unit 270 reads a series of computer programs stored in the memory 250 to complete the target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization.

[0156] In addition, the present application can also be implemented through hardware circuits or hardware circuits combined with software. Therefore, the implementation of the present application is not limited to any specific hardware circuits, software, or a combination of the two.

[0157] See also Figure 15 In an embodiment of the present application, an electronic device 4000 is provided, and the electronic device 4000 may include: (needs to be adaptively modified according to the specific circumstances of the present application) a desktop computer, a laptop computer, a server, etc.

[0158] exist Figure 15 In the embodiment, the electronic device 4000 includes at least one processor 4001, at least one communication bus 4002 and at least one memory 4003.

[0159] The processor 4001 and the memory 4003 are connected, for example, via a communication bus 4002. Optionally, the electronic device 4000 may further include a transceiver 4004, which may be used for data exchange between the electronic device and other electronic devices, such as data transmission and / or data reception. It should be noted that in actual applications, the number of transceivers 4004 is not limited to one, and the structure of the electronic device 4000 does not constitute a limitation on the embodiments of the present application.

[0160] Processor 4001 may be a CPU (Central Processing Unit), a general-purpose processor, a DSP (Digital Signal Processor), an ASIC (Application Specific Integrated Circuit), an FPGA (Field Programmable Gate Array), or other programmable logic devices, transistor logic devices, hardware components, or any combination thereof. It may implement or execute the various exemplary logic blocks, modules, and circuits described in conjunction with the disclosure of this application. Processor 4001 may also be a combination that implements computing functions, such as a combination of one or more microprocessors, a combination of a DSP and a microprocessor, and the like.

[0161] The communication bus 4002 may include a path for transmitting information between the above components. The communication bus 4002 may be a PCI (Peripheral Component Interconnect) bus or an EISA (Extended Industry Standard Architecture) bus. The communication bus 4002 may be divided into an address bus, a data bus, a control bus, etc. For ease of representation, Figure 10 Only one thick line is used in the diagram, but this does not mean that there is only one bus or one type of bus.

[0162] The memory 4003 can be a ROM (Read Only Memory) or other types of static storage devices that can store static information and instructions, a RAM (Random Access Memory) or other types of dynamic storage devices that can store information and instructions, or an EEPROM (Electrically Erasable Programmable Read Only Memory), a CD-ROM (Compact Disc Read Only Memory) or other optical disk storage, optical disk storage (including compact disc, laser disc, optical disc, digital versatile disc, Blu-ray disc, etc.), a magnetic disk storage medium or other magnetic storage device, or any other medium that can be used to carry or store desired program code in the form of instructions or data structures and can be accessed by a computer, but is not limited to these.

[0163] The memory 4003 stores a computer program, and the processor 4001 reads the computer program stored in the memory 4003 through the communication bus 4002 .

[0164] When the computer program is executed by the processor 4001, the target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization in the above embodiments is implemented.

[0165] In addition, a storage medium is provided in an embodiment of the present application, on which a computer program is stored. When the computer program is executed by a processor, the target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization in the above-mentioned embodiments is implemented.

[0166] In an embodiment of the present application, a computer program product is provided, comprising a computer program stored in a storage medium. A processor of a computer device reads the computer program from the storage medium and executes the computer program, causing the computer device to perform the target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization described in each of the above embodiments.

[0167] Compared with the related art, the beneficial effects of this application are:

[0168] 1. This application proposes a target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization. This application first obtains a binocular image of the target, which includes at least the rotation matrix, displacement vector and geometric features of the target. The rotation matrix is ​​decomposed into a rotation vector and a rotation angle by a rotation matrix decomposition method. The rotation vector and the rotation angle are then uniformly sampled to ensure that all-round posture changes are covered. The mathematical relationship between the rotation matrix and the displacement vector is established based on the geometric properties of the geometric features. Finally, the target pose is estimated by designing an objective function and applying an optimization algorithm. The particle swarm optimization algorithm and the nonlinear optimization algorithm can simultaneously process the three basic geometric features of points, lines and circles, and there is no limit on the number of features and the relationship between their positions. It has a very high accuracy and robustness, thereby solving the problems of low accuracy, poor versatility and robustness in the prior art.

[0169] 2. This application proposes an algorithm for estimating the six-degree-of-freedom position and attitude (pose) of a target based on stereo vision and particle swarm optimization. The algorithm utilizes the basic geometric features (circles and straight lines) on the target and accurately calculates the target pose according to the projection equations of the basic geometric features. It is not limited by the number of geometric features and their mutual positional relationships. Experiments show that the pose estimated by this application is very accurate, providing reliable pose information for downstream tasks such as assembly and docking.

[0170] 3. This application uses particle swarm optimization algorithm and nonlinear optimization algorithm to simultaneously process three basic geometric features: points, lines, and circles. There is no limit on the number of features and the relationship between their positions, and it has very high accuracy and robustness.

[0171] 4. This application proposes a particle swarm optimization algorithm based on stereo vision to realize the estimation of the relative pose of an object. First, the rotation matrix is ​​decomposed into a rotation vector and a rotation angle through the Rodriguez formula, and the Thomson method is used to uniformly sample the rotation vector on the unit sphere, and the rotation angle is uniformly sampled between π and π. Then, the mathematical relationship between the rotation matrix and the displacement vector is established through the geometric characteristics of the basic geometric features in the stereo camera itself. Then, by combining the particle swarm optimization algorithm with our own designed objective function, the pose of the object can be estimated. Finally, the pose estimated by the particle swarm optimization algorithm is optimized using the Levenberg-Marquardt method to obtain a high-precision and high-robust pose estimation.

[0172] It should be understood that although the steps in the flowcharts of the accompanying drawings are shown in sequence as indicated by the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order restriction on the execution of these steps, and they can be executed in other orders. Moreover, at least some of the steps in the flowcharts of the accompanying drawings may include multiple sub-steps or multiple stages, and these sub-steps or stages are not necessarily executed at the same time, but can be executed at different times, and their execution order is not necessarily sequential, but can be executed in turn or alternately with other steps or at least a portion of the sub-steps or stages of other steps.

[0173] The above content is only a preferred exemplary embodiment of the present application and is not intended to limit the implementation scheme of the present application. Ordinary technicians in this field can easily make corresponding changes or modifications based on the main ideas and spirit of the present application. Therefore, the scope of protection of the present application shall be based on the scope of protection required by the claims.

Claims

1. A target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization, characterized in that: The method comprises: Acquire a binocular image of the target; the binocular image includes at least a rotation matrix, a displacement vector, and geometric features of the target; Decomposing the rotation matrix into a rotation vector and a rotation angle by a rotation matrix decomposition method, and uniformly sampling the rotation vector and the rotation angle with six degrees of freedom to obtain a data point set; Establishing a mathematical relationship between the rotation matrix and the displacement vector based on the geometric properties of the geometric feature and the data point set; By designing an objective function and applying an optimization algorithm, the target pose is estimated on the binocular image of the target to obtain a pose estimation result; the objective function is used to estimate the pose of the target; the optimization algorithm is used to optimize the pose estimation result; the pose estimation result is used to display the six-degree-of-freedom pose of the target and determine the position and orientation of the target; The establishing of a mathematical relationship between the rotation matrix and the displacement vector according to the geometric properties of the geometric feature and the data point set comprises: Calculating the geometric distance between the projection point of the target on the binocular image and the data point set by sampling the Sampson distance; the geometric distance is a geometric property of a geometric feature; the geometric feature includes a point, a line and a surface; Establishing a mathematical relationship between the rotation matrix and the displacement vector according to the geometric distance from the projection point to the data point set; The optimization algorithm includes a particle swarm optimization algorithm; The target pose estimation is performed on the binocular image of the target by designing an objective function and applying an optimization algorithm to obtain a pose estimation result, including: Designing an objective function based on the geometric relationship and rotational posture of the binocular images in stereo vision; the objective function includes constraints on the geometric relationship and rotational posture in stereo vision; Searching for an optimal rotation matrix and displacement vector in a parameter space between the rotation matrix and the displacement vector by a particle swarm optimization algorithm, and minimizing the objective function according to the found optimal rotation matrix and displacement vector; According to the objective function minimized by the particle swarm optimization algorithm, the relative pose of the target relative to the target coordinate system is estimated to obtain the pose estimation result; the target coordinate system is used to quantify the pose change of the target; The design objective function includes: Parameterizing the rotation matrix of the target to obtain geometric distances of the straight line and circle geometric features of the target in the binocular image; A corresponding Sampson distance is obtained according to the geometric distance, and an objective function is constructed according to the Sampson distance.

2. The method according to claim 1, wherein Decomposing the rotation matrix into a rotation vector and a rotation angle by a rotation matrix decomposition method includes: The rotation matrix is ​​decomposed into a rotation vector and a rotation angle using the Rodriguez formula.

3. The method according to claim 1, wherein The uniform sampling of the rotation vector and the rotation angle in six degrees of freedom to obtain a data point set includes: The rotation vector is uniformly sampled in six degrees of freedom on the unit sphere using the Thomson method; The rotation angles between 0 and π are uniformly sampled to obtain a data point set, ensuring that the data point set includes posture changes of the target in six degrees of freedom; the six degrees of freedom include three translation degrees of freedom and three rotation degrees of freedom.

4. The method according to claim 1, wherein After obtaining the pose estimation result, the method further includes: The relative pose of the target in the pose estimation result is optimized by the Levenberg-Marquardt algorithm.

5. A target six-degree-of-freedom pose estimation device based on binocular vision and particle swarm optimization, characterized in that: The device is used to implement the target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization as claimed in claim 1, comprising: A data acquisition module is used to acquire a binocular image of the target; the binocular image includes at least a rotation matrix, a displacement vector and geometric features of the target; a uniform sampling module, configured to decompose the rotation matrix into a rotation vector and a rotation angle by a rotation matrix decomposition method, and perform uniform sampling of the rotation vector and the rotation angle in six degrees of freedom to obtain a data point set; a mathematical relationship building module, configured to establish a mathematical relationship between the rotation matrix and the displacement vector based on the geometric properties of the geometric feature and the data point set; The pose estimation module is used to estimate the target pose of the binocular image of the target by designing an objective function and applying an optimization algorithm to obtain a pose estimation result.

6. An electronic device, characterized in that: include: at least one processor and at least one memory, wherein: The memory stores program instructions or codes; The program instructions or codes are loaded and executed by the processor, so that the electronic device implements the target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization as described in any one of claims 1 to 4.

7. A storage medium having program instructions or codes stored thereon, characterized in that: The program instructions or codes are loaded and executed by a processor to implement the target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization as described in any one of claims 1 to 4.

Images