WO2025123370A1 - 基于双目视觉与粒子群优化的目标六自由度位姿估计方法 - Google Patents

基于双目视觉与粒子群优化的目标六自由度位姿估计方法 Download PDF

Info

Publication number
WO2025123370A1
WO2025123370A1 PCT/CN2023/139298 CN2023139298W WO2025123370A1 WO 2025123370 A1 WO2025123370 A1 WO 2025123370A1 CN 2023139298 W CN2023139298 W CN 2023139298W WO 2025123370 A1 WO2025123370 A1 WO 2025123370A1
Authority
WO
WIPO (PCT)
Prior art keywords
target
rotation
freedom
rotation matrix
geometric
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
PCT/CN2023/139298
Other languages
English (en)
French (fr)
Inventor
陈思凯
刘畅
刘嘉
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Institute of Advanced Technology of CAS
Original Assignee
Shenzhen Institute of Advanced Technology of CAS
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shenzhen Institute of Advanced Technology of CAS filed Critical Shenzhen Institute of Advanced Technology of CAS
Priority to PCT/CN2023/139298 priority Critical patent/WO2025123370A1/zh
Publication of WO2025123370A1 publication Critical patent/WO2025123370A1/zh
Anticipated expiration legal-status Critical
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods

Definitions

  • 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.
  • Target relative position and attitude estimation has a large number of applications in aerospace and industrial assembly.
  • the relative attitude estimation between two spacecraft is a key issue in space rendezvous and docking.
  • the stellar probe needs a high reliability, high autonomy and high precision relative position and attitude estimation algorithm to achieve the landing task.
  • the premise of realizing the task of automatic assembly is to estimate the position and attitude of the object relative to the camera.
  • the pose estimation methods can be roughly divided into five categories: point-based, template-based, segmentation-based, edge-based, and basic geometric feature-based.
  • the pose estimation methods in the existing technologies all limit the number of basic geometric features and require the geometric restrictions between the basic geometric features as prior knowledge, resulting in poor versatility of the methods.
  • some basic geometric features are affected by lighting and occlusion and cannot be imaged in the image, resulting in poor robustness of the methods.
  • 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 the related art.
  • a method for estimating the six-degree-of-freedom pose of a target based on binocular vision and particle swarm optimization comprising acquiring a binocular image of the target; the binocular image at least comprising 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, 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.
  • decomposing the rotation matrix into a rotation vector and a rotation angle by 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 Rodriguez formula.
  • 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 a unit sphere by 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 translational degrees of freedom and three rotational degrees of freedom.
  • the mathematical relationship between the rotation matrix and the displacement vector is established by the following steps: the geometric distance from the projection point of the target on the binocular image to the data point set is calculated by sampling the Sampson distance; the geometric distance is the geometric property of the geometric features; the geometric features include points, lines and surfaces.
  • the mathematical relationship between the rotation matrix and the displacement vector is established according to the geometric distance from the projection point to the data point set.
  • 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 stereoscopic vision; the objective function contains constraints on the geometric relationship and rotation posture in stereoscopic vision; searching for the optimal rotation matrix and displacement vector in the parameter space between the rotation matrix and the displacement vector by the particle swarm optimization algorithm, and according to the found optimal rotation matrix and displacement vector The objective function is minimized by the particle swarm optimization algorithm; the relative posture of the target relative to the target coordinate system is estimated according to the objective function minimized by the particle swarm optimization algorithm to obtain the posture estimation result; the target coordinate system is used to quantify the posture change of the target.
  • the objective function is designed by the following steps: 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.
  • 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.
  • a target six-degree-of-freedom pose estimation device based on binocular vision and particle swarm optimization, the device comprising a data acquisition module, used to acquire a binocular image of the target; the binocular image at least includes a rotation matrix, a displacement vector and a geometric feature of the target; a uniform sampling module, used to decompose the rotation matrix into a rotation vector and a rotation angle by a rotation matrix decomposition method, and perform 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, 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; a pose estimation module, used to perform 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.
  • a data acquisition module used to acquire a binocular image of the target
  • the binocular image at least includes a rotation matrix, a displacement vector
  • 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.
  • a storage medium stores program instructions or codes thereon, and 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 above.
  • a computer program product includes program instructions or codes, the program instructions or codes are stored in a storage medium, and a 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.
  • the present application solves the problems of low accuracy, poor versatility and robustness in the related technology.
  • 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 by 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.
  • the mathematical relationship between the rotation matrix and the displacement vector is established based on the geometric properties of the geometric features and the data point set.
  • 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.
  • 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 relationship between the number of features and their relative positions. It has a very high accuracy and robustness, thereby solving the problems of low accuracy, poor versatility and poor robustness in the prior art.
  • FIG1 is a schematic diagram of an implementation environment involved in the present application.
  • FIG. 2 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
  • FIG3 is a flow chart of step 270 in one embodiment of the embodiment corresponding to FIG2 ;
  • FIG4 is a schematic diagram of projection of a point on a target coordinate system in stereoscopic vision in an exemplary embodiment
  • FIG5 is a schematic diagram of a design objective function in an exemplary embodiment
  • FIG6 is a schematic diagram of constructing a mathematical relationship between a rotation matrix and a displacement vector in an exemplary embodiment
  • FIG7 is a schematic diagram of processing a circle feature in an exemplary embodiment
  • FIG8 is a schematic diagram of target pose optimization in an exemplary embodiment
  • FIG9 is a line graph showing comparative test results of an exemplary embodiment and a prior art
  • FIG10 is a line graph showing comparative test results of an exemplary embodiment and another prior art
  • FIG11 is a line graph of experimental results of the number of straight line features and circle features of an exemplary embodiment
  • FIG12 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;
  • FIG. 13 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
  • FIG14 is a schematic diagram showing the structure of a server according to an exemplary embodiment
  • Fig. 15 is a block diagram of an electronic device according to an exemplary embodiment.
  • the acquisition terminal 110 and the server terminal 130 may be connected to each other by wired or wireless communication to achieve data transmission between the two.
  • the transmitted data may be a binocular image.
  • the server end 130 can also be considered as a cloud, a cloud platform, a platform end, a service end, etc.
  • the server end 130 can be a server, a server cluster composed of multiple servers, or a cloud computing center composed of multiple servers, so as to better provide background services to the massive acquisition end 110.
  • the background service includes a target six-degree-of-freedom pose estimation service based on binocular vision and particle swarm optimization.
  • 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 based on binocular vision and particle swarm optimization based on the rotation matrix, displacement vector and geometric features of the target.
  • the server 130 After the server 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 a 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, and then establishes 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 finally estimates 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.
  • An 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 an electronic device, which can be the server end 130 in the implementation environment shown in Figure 1, or a desktop computer, a laptop computer, a server, etc.
  • the method may include the following steps:
  • Step 210 acquiring a binocular image of the target.
  • 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, 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.
  • the binocular image includes at least a rotation matrix, a displacement vector, and geometric features of the target.
  • the binocular image of the target can 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.
  • corresponding software or programming languages such as the OpenCV library in Python
  • Step 230 decomposing the rotation matrix into a rotation vector and a rotation angle by a rotation matrix decomposition method Degrees, the rotation vector and rotation angle are uniformly sampled in six degrees of freedom to obtain a data point set.
  • the rotation matrix is decomposed into a rotation vector and a rotation angle by using the Rodriguez formula.
  • 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 fonts represent scalars; bold fonts represent matrices or vectors; R and t represent rotation matrices and displacement vectors respectively; I represents the three-dimensional unit matrix.
  • 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 six degrees of freedom, wherein the six degrees of freedom include three translational degrees of freedom and three rotational degrees of freedom.
  • the embodiment of the present application uses the Thomson method to uniformly sample the rotation vector on the unit sphere.
  • the Thomson method aims to minimize the electric potential energy of the system.
  • the sampled rotation 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.
  • a stereo camera model is constructed for the target according to the left camera and the right camera.
  • 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 are parallel to l X, l Y, r X, r Y respectively.
  • a horizontal line above the vector symbol indicates the projection of the basic geometric features represented by this vector in the image, for example: represents the projection of o p on the left image.
  • l K 12 and r K 12 represent the matrices formed by the first two rows of l K and r K, respectively
  • l K 3 and r K 3 represent the matrices formed by the third rows of l K and r K, respectively
  • l R o and l t o represent 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 represent the rotation matrix and displacement vector of the target coordinate system relative to the left camera coordinate system.
  • the embodiment of the present application constructs a stereo camera model of the target, and obtains the depth information of the target in the scene by simulating the human binocular vision system. It can infer the distance of the target from the camera, provide more accurate object positioning information, and make up for the defect that the monocular camera may not be able to accurately estimate the distance of the object in the absence of depth information. It improves the accuracy of depth perception and is more robust to environments with changing lighting and lack of texture.
  • Step 250 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.
  • step 250 may include the following steps:
  • Step 251 calculates the geometric distance from the projection point of the target on the binocular image to the data point set by sampling the Sampson distance.
  • the geometric distance is the geometric property of the geometric features, and the geometric features include points, lines and surfaces.
  • Step 253 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.
  • Step 270 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 target's posture
  • the optimization algorithm is used to optimize the posture estimation results
  • the posture estimation results are used to display the target's six-degree-of-freedom posture and determine the target's position and orientation.
  • the optimization algorithm includes a particle swarm optimization algorithm.
  • step 270 may include the following steps:
  • Step 310 designing an objective function based on the geometric relationship and rotational posture of the binocular images in stereoscopic vision.
  • the objective function is designed by first parameterizing the rotation matrix of the target, obtaining the geometric distance of the straight line and circle geometric features of the target 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.
  • 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
  • NL and NL represent the number of data points of the line and the ellipse on the image respectively
  • d i is calculated as follows:
  • q i represents the i-th straight line data point, represents the point on the line segment that is closest to q i .
  • ⁇ and ⁇ * are introduced to avoid data points falling on the line where the image of the model straight line segment is located, because accurate pose estimation should make the data points fall on the image of the model straight line segment.
  • ⁇ represents a non-zero constant
  • K represents the intrinsic parameter matrix of the camera (represented by l K or r K in the embodiment of the present application);
  • ⁇ 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 spatial circle;
  • C represents the coordinates of the spatial circle in the camera coordinate system;
  • r represents the radius of the spatial circle.
  • FIG. 10 is a line graph showing comparative test results of an exemplary embodiment and another prior art.
  • FIG. 11 is a line graph showing the experimental results of the number of straight line features and circle features of an exemplary embodiment.
  • 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 both decrease.
  • 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 both decrease.
  • the embodiment of the present application improves the accuracy of the rotation matrix and the displacement vector, and improves the accuracy of the displacement vector estimation. It can simultaneously process three basic geometric features of points, lines, and circles, and there is no restriction 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 existing in the prior art.
  • FIG13 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.
  • FIG13-1 the embodiment of the present application carried out a physical experiment on model 1
  • FIG(a) is a left camera image
  • FIG(b) is a right camera image
  • the circular lines are images of model 1 projected into each image according to the posture estimated by the embodiment of the present application
  • FIG13-2 the embodiment of the present application carried out a physical experiment on model 2
  • FIG(a) is a left camera image
  • FIG(b) is a right camera image
  • the circular lines are images of model 2 projected into each image according to the posture estimated by the embodiment of the present application
  • FIG13-3 the embodiment of the present application carried out a physical experiment on model 3
  • FIG(a) is a left camera image
  • FIG(b) is a right camera image
  • the circular lines are images of model 2 projected into each image according to the posture estimated by the embodiment of the present application.
  • Figure (a) is the left camera image
  • Figure (b) is the right camera image.
  • the circular lines are the images of the 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 in the present application is very close to the real posture.
  • the following is an embodiment of the device of the present application, which can be used to execute the target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization involved in the present application.
  • the method embodiment of the target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization involved in 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 the present application.
  • An embodiment of the present application provides a target six-degree-of-freedom pose estimation device 800 based on binocular vision and particle swarm optimization, 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.
  • the data acquisition module 810 is used to acquire a binocular image of the target; the binocular image at least includes a rotation matrix, a displacement vector and geometric features of the target.
  • 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 six-degree-of-freedom uniform sampling on the rotation vector and the rotation angle to obtain a data point set.
  • the mathematical relationship building module 850 is used to establish the mathematical relationship between the rotation matrix and the displacement vector according to the geometric properties of the geometric features and the data point set.
  • 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.
  • the target six-degree-of-freedom pose estimation device based on binocular vision and particle swarm optimization only uses the division of the above-mentioned functional modules as an example when performing the target six-degree-of-freedom pose estimation based on binocular vision and particle swarm optimization.
  • 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.
  • the target six-degree-of-freedom pose estimation device based on binocular vision and particle swarm optimization provided in the above-mentioned 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 way in which each module performs the operation has been described in detail in the method embodiment and will not be repeated here.
  • Fig. 14 is a schematic diagram of the structure of a server according to an exemplary embodiment.
  • the server is applicable to the server end 130 in the implementation environment shown in Fig. 1 .
  • server is only an example adapted to the present application and cannot be considered to provide any limitation on the scope of use of the present application.
  • the server cannot be interpreted as needing to rely on or having to have one or more components in the exemplary server 2000 shown in FIG. 14.
  • the hardware structure of the server 2000 may vary greatly due to different configurations or performances.
  • the server 2000 includes: a power supply 210 , an interface 230 , at least one memory 250 , and at least one central processing unit (CPU, Central Processing Units) 270 .
  • CPU Central Processing Unit
  • the power supply 210 is used to provide operating voltage for each hardware device on the server 2000 .
  • the interface 230 includes at least one wired or wireless network interface, and is used to interact with external devices, such as the interaction between the terminal 100 and the server 200 in the implementation environment shown in FIG. 1 .
  • 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, as shown in FIG. 14, and this is not specifically limited here.
  • the memory 250 is a carrier for storing resources, which may be a read-only memory, a random access memory, a disk or an optical disk, etc.
  • the resources stored thereon include an operating system 251, an application 253 and data 255, etc.
  • the storage method may be temporary storage or permanent storage.
  • the operating system 251 is used to manage and control various hardware devices and application programs 253 on the server 200, so as to enable the central processing unit 270 to calculate and process the massive data 255 in the memory 250. It can be Windows ServerTM, Mac OS XTM, UnixTM, LinuxTM, FreeBSDTM, etc.
  • the application 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 (not shown in FIG. 14 ), each of which may include a computer program for the server 2000.
  • a target six-degree-of-freedom pose estimation device based on binocular vision and particle swarm optimization may be regarded as an application 253 deployed on the server 2000.
  • the data 255 may be a binocular image stored in a binocular camera, etc., and stored in the memory 250 .
  • the central processor 270 may include one or more processors and is configured to communicate with the memory 250 through at least one communication bus to read the computer program stored in the memory 250, thereby realizing the operation and processing of the mass data 255 in the memory 250. For example, the central processor 270 reads a series of computer programs stored in the memory 250 to complete the binocular vision-based Objective six-degree-of-freedom pose estimation method with particle swarm optimization.
  • 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.
  • An electronic device 4000 is provided in an embodiment of the present application.
  • 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.
  • the electronic device 4000 includes at least one processor 4001 , at least one communication bus 4002 , and at least one memory 4003 .
  • the electronic device 4000 may further include a transceiver 4004, which may be used for data interaction between the electronic device and other electronic devices, such as data transmission and/or data reception.
  • a transceiver 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.
  • Processor 4001 can 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 can implement or execute various exemplary logic blocks, modules and circuits described in conjunction with the disclosure of this application. Processor 4001 can 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, etc.
  • 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, etc.
  • the communication bus 4002 may be divided into an address bus, a data bus, a control bus, etc.
  • FIG10 only uses a thick line, but it does not mean that there is only one bus or one type of bus.
  • the memory 4003 may 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 a CD-ROM (Compact Disc Read Only Memory).
  • ROM Read Only Memory
  • RAM Random Access Memory
  • EEPROM Electrically Erasable Programmable Read Only Memory
  • CD-ROM Compact Disc Read Only Memory
  • CD-ROM Compact Disc Read Only Memory
  • CD-ROM Compact Disc Read Only Memory
  • CD-ROM Compact Disc Read Only Memory
  • the memory 4003 stores a computer program
  • the processor 4001 reads the computer program stored in the memory 4003 through the communication bus 4002 .
  • the target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization in the above-mentioned embodiments is implemented.
  • a storage medium is provided in an embodiment of the present application, on which a computer program is stored.
  • 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.
  • a computer program product which includes a computer program stored in a storage medium.
  • a processor of a computer device reads the computer program from the storage medium, and the processor executes the computer program, so that the computer device executes the target six-degree-of-freedom pose estimation method based on binocular vision and particle swarm optimization in the above-mentioned embodiments.
  • This application proposes a method for estimating the six-degree-of-freedom pose of a target 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, and then the rotation vector and the rotation angle are uniformly sampled to ensure that all-round posture changes are covered.
  • a mathematical relationship between the rotation matrix and the displacement vector is established.
  • the target pose is estimated by designing an objective function and applying an optimization algorithm.
  • the three basic geometric features of points, lines and circles can be processed simultaneously, and there is no limit on the relationship between the number of features and their positions. It has a very high accuracy and robustness, thereby solving the problems of low accuracy, poor versatility and poor robustness in the prior art.
  • This application proposes a target six-degree-of-freedom position and attitude (pose) estimation algorithm based on stereo vision and particle swarm optimization.
  • the algorithm uses the basic geometric features (circles, straight lines) on the target and accurately calculates the target pose according to the projection equation of the basic geometric features. It is not limited by the number of geometric features and the position relationship between them.
  • the pose estimated by this application is very accurate and provides a good basis for downstream tasks. For example, assembly and docking provide reliable posture information.
  • 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.
  • This application proposes a particle swarm optimization algorithm based on stereo vision to estimate the relative pose of an object.
  • 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 ⁇ .
  • the mathematical relationship between the rotation matrix and the displacement vector is established through the geometric properties of the basic geometric features in the stereo camera.
  • the particle swarm optimization algorithm is combined with the objective function designed by our to estimate the pose of the object.
  • 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.

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Image Analysis (AREA)

Abstract

本申请实施例公开了一种基于双目视觉与粒子群优化的目标六自由度位姿估计方法和装置，涉及计算机视觉领域，其中，所述方法包括：获取目标的双目图像；所述双目图像中至少包括目标的旋转矩阵、位移向量和几何特征；通过旋转矩阵分解方法将所述旋转矩阵分解为旋转向量和旋转角度，对所述旋转向量和旋转角度进行六自由度的均匀采样，得到数据点集；根据所述几何特征的几何性质和所述数据点集，建立所述旋转矩阵与位移向量之间的数学关系；通过设计目标函数和应用优化算法，对所述目标的双目图像进行目标位姿估计，得到位姿估计结果。本申请解决了相关技术中存在的准确率较低、泛用性和鲁棒性差的问题。

Description

基于双目视觉与粒子群优化的目标六自由度位姿估计方法 技术领域
本申请涉及计算机视觉领域，尤其涉及一种基于双目视觉与粒子群优化的目标六自由度位姿估计方法。
背景技术
目标相对位姿估计在航天、工业装配中有大量应用，在航天领域，两艘航天器之间的相对姿态估计是空间交会对接的关键问题。在行星探测方面，星体探测器在着陆过程中需要高可靠性、高自主性和高精度的相对位置和姿态估计算法来实现着落任务。在工业领域，实现自动装配的工作任务的前提是需要估计物件相对摄像机的位姿。
根据选取图像特征的不同，可以将姿态估计的方法大致分为五类：基于点、基于模板、基于分割、基于边缘、基于基本几何特征，研究人员已经注意到，目标物体上明确的基本几何特征，例如3-D直线段和3-D圆，具有良好的鲁棒性。
然而，现有技术中的位姿估计方法都限制了基本几何特征的数量并需要基本几何特征之间的几何限制作为先验知识，导致方法的泛用性差，此外，在现实应用场景中，部分基本几何特征受到光照、遮挡的影响，在图像中无法成像，导致方法的鲁棒性差。
因此，急需一种准确率高、泛用性和鲁棒性高的基于双目视觉与粒子群优化的目标六自由度位姿估计方法。
发明内容
本申请各实施例提供一种基于双目视觉与粒子群优化的目标六自由度位姿估计方法、装置、电子设备及存储介质，以解决相关技术中存在的准确率较低、泛用性和鲁棒性差的问题。
为解决上述技术问题，本申请所采用的技术方案为：
根据本申请的一个方面，一种基于双目视觉与粒子群优化的目标六自由度位姿估计方法，所述方法包括获取目标的双目图像；所述双目图像中至少包括目标的旋转矩阵、位移向量和几何特征；通过旋转矩阵分解方法将所述旋转矩阵分解为旋转向量和旋转角度，对所述旋转向量和旋转角度进行六自由度的均匀采样，得到数据点集；根据所述几何特征的几何性质和所述数据点集，建立所述旋转矩阵与位移向量之间的数学关系；通过设计目标函数和应用优化算法，对所述目标的双目图像进行目标位姿估计，得到位姿估计结果；所述目标函数用于估计目标的姿态；所述优化算法用于优化位姿估计结果；所述位姿估计结果用于展示目标的六自由度姿态以及确定目标的位置和朝向。
在一示例性实施例，通过旋转矩阵分解方法将所述旋转矩阵分解为旋转向量和旋转角度通过以下步骤实现：通过罗德里格斯公式将旋转矩阵分解为旋转向量和旋转角度。
在一示例性实施例，对所述旋转向量和旋转角度进行六自由度的均匀采样，得到数据点集通过以下步骤实现：通过汤姆逊方法在单位球面对旋转向量进行六自由度的均匀采样；对0到π之间的旋转角度进行均匀采样，得到数据点集，确保所述数据点集中包括目标在六个自由度上的姿态变化；所述六个自由度包括三个平移自由度和三个旋转自由度。
在一示例性实施例，根据所述几何特征的几何性质和所述数据点集，建立所述旋转矩阵与位移向量之间的数学关系通过以下步骤实现：通过采样桑普森距离计算目标在所述双目图像上的投影点到数据点集的几何距离；所述几何距离为几何特征的几何性质；所述几何特征包括点、直线和面。根据所述投影点到数据点集的几何距离建立所述旋转矩阵与位移向量之间的数学关系。
在一示例性实施例，优化算法包括粒子群优化算法；通过设计目标函数和应用优化算法，对所述目标的双目图像进行目标位姿估计，得到位姿估计结果通过以下步骤实现：基于所述双目图像在立体视觉中的几何关系和旋转姿态设计目标函数；所述目标函数包含立体视觉中的几何关系和旋转姿态的约束条件；通过粒子群优化算法在所述旋转矩阵与位移向量之间的参数空间中寻找最优的旋转矩阵和位移向量，根据寻找到的最优的旋转矩阵和位移向 量对所述目标函数进行最小化；根据所述粒子群优化算法最小化后的目标函数，估计目标相对于目标坐标系的相对位姿，得到所述位姿估计结果；所述目标坐标系用于量化目标的位姿变化。
在一示例性实施例，设计目标函数通过以下步骤实现：将所述目标的旋转矩阵参数化，得到目标的直线和圆的几何特征在所述双目图像中的几何距离；根据所述几何距离得到对应的桑普森距离，根据所述桑普森距离构建目标函数。
在一示例性实施例，得到所述位姿估计结果之后，所述方法还包括以下步骤：通过列文伯格马夸特算法对所述位姿估计结果中所述目标的相对位姿进行优化。
根据本申请的一个方面，一种基于双目视觉与粒子群优化的目标六自由度位姿估计装置，所述装置包括数据获取模块，用于获取目标的双目图像；所述双目图像中至少包括目标的旋转矩阵、位移向量和几何特征；均匀采样模块，用于通过旋转矩阵分解方法将所述旋转矩阵分解为旋转向量和旋转角度，对所述旋转向量和旋转角度进行六自由度的均匀采样，得到数据点集；数学关系构建模块，用于根据所述几何特征的几何性质和所述数据点集，建立所述旋转矩阵与位移向量之间的数学关系；位姿估计模块，用于通过设计目标函数和应用优化算法，对所述目标的双目图像进行目标位姿估计，得到位姿估计结果。
根据本申请的一个方面，一种电子设备，包括至少一个处理器以及至少一个存储器，其中，所述存储器上存储有程序指令或代码；所述程序指令或代码被所述处理器加载并执行，使得电子设备实现如上所述的基于双目视觉与粒子群优化的目标六自由度位姿估计方法。
根据本申请的一个方面，一种存储介质，其上存储有程序指令或代码，所述程序指令或代码被处理器加载并执行，以实现如上所述的基于双目视觉与粒子群优化的目标六自由度位姿估计方法。
根据本申请的一个方面，一种计算机程序产品，计算机程序产品包括程序指令或代码，程序指令或代码存储在存储介质中，电子设备的处理器从存储介质读取程序指令或代码，加载并执行该程序指令或代码，使得电子设备实现如上所述的基于双目视觉与粒子群优化的目标六自由度位姿估计方法。
本申请提供的技术方案带来的有益效果是：
在上述技术方案中，本申请解决了相关技术中准确率较低、泛用性和鲁棒性差的问题。
具体而言，本申请首先获取目标的双目图像，双目图像中至少包括目标的旋转矩阵、位移向量和几何特征，通过旋转矩阵分解方法将旋转矩阵分解为旋转向量和旋转角度，然后对旋转向量和旋转角度进行六自由度的均匀采样，得到数据点集，确保全方位的姿态变化被覆盖，再根据几何特征的几何性质和数据点集，建立旋转矩阵与位移向量之间的数学关系，最后通过设计目标函数和应用优化算法对目标的双目图像进行目标位姿估计，得到位姿估计结果，通过粒子群优化算法及非线性优化算法，可以同时处理点、直线、圆三种基本几何特征，并且特征数量与相互间位置的关系没有限制，具有相当高的准确性、鲁棒性，进而解决了现有技术中存在的准确率较低、泛用性和鲁棒性差的问题。
应当理解的是，以的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本申请。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本申请的实施例，并于说明书一起用于解释本申请的原理。
图1是根据本申请所涉及的实施环境的示意图；
图2是根据一示例性实施例示出的一种基于双目视觉与粒子群优化的目标六自由度位姿估计方法的流程图；
图3是图2对应实施例中步骤270在一个实施例的流程图；
图4是一示例性实施例中目标坐标系上的点在立体视觉中的投影示意图；
图5是一示例性实施例中设计目标函数的示意图；
图6是一示例性实施例中构建旋转矩阵与位移向量之间的数学关系的示意图；
图7是一示例性实施例中处理圆特征的示意图；
图8是一示例性实施例中目标位姿优化的示意图；
图9是一示例性实施例与一现有技术的对比试验结果折线图；
图10是一示例性实施例与另一现有技术的对比试验结果折线图；
图11是一示例性实施例的直线特征和圆特征数量实验结果折线图；
图12是一应用场景中一种基于双目视觉与粒子群优化的目标六自由度位姿估计方法在模型1、模型2和模型3中的实现结果示意图；
图13是根据一示例性实施例示出的一种基于双目视觉与粒子群优化的目标六自由度位姿估计装置的框图；
图14是根据一示例性实施例示出的一种服务器的结构示意图；
图15是根据一示例性实施例示出的一种电子设备的框图。
具体实施方式
下面详细描述本申请的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本申请，而不能解释为对本申请的限制。
本技术领域技术人员可以理解，除非特意声明，这里使用的单数形式“一”、“一个”、“所述”和“该”也可包括复数形式。应该进一步理解的是，本申请的说明书中使用的措辞“包括”是指存在所述特征、整数、步骤、操作、元件和/或组件，但是并不排除存在或添加一个或多个其他特征、整数、步骤、操作、元件、组件和/或它们的组。应该理解，当我们称元件被“连接”或“耦接”到另一元件时，它可以直接连接或耦接到其他元件，或者也可以存在中间元件。此外，这里使用的“连接”或“耦接”可以包括无线连接或无线耦接。这里使用的措辞“和/或”包括一个或更多个相关联的列出项的全部或任一单元和全部组合。
下面是对本申请涉及的几个名词进行的介绍和解释：
双目视觉(Binocular Vision)：是指通过两只眼睛同时观察同一场景，以获取深度和立体感的一种视觉方式，每只眼睛拍摄到的图像视角略有不同，这种差异被称为视差(disparity)，由大脑解释为场景中物体的深度信息，双目视觉是人类和许多其他脊椎动物的一种常见的深度感知机制。
粒子群优化(Particle Swarm Optimization，PSO)：是一种元启发式算法， 灵感来源于鸟群或鱼群等群体行为，该算法最初由Russell C.Eberhart和James Kennedy于1995年提出，PSO的基本思想是通过模拟群体中个体之间的合作与竞争关系来搜索最优解，在PSO中，问题的解被称为粒子，这些粒子在问题的解空间中搜索最优解，每个粒子具有一个位置和速度，通过根据其自身历史最优位置和群体历史最优位置，以及当前位置和速度，来调整自身的移动方向和速度，这样，整个粒子群通过不断地调整位置和速度在解空间中找到最优解，PSO的性能受到参数设置的影响，包括粒子数量、惯性权重、加速因子等，合适的参数设置可以使算法更好地收敛到全局最优解，PSO广泛应用于函数优化、神经网络训练、图像处理等领域。
目标六自由度位姿估计：是指对物体或目标在空间中的位置和姿态(位姿)进行六自由度的估计，在三维空间中，物体的六自由度姿态包括三个平移自由度(沿x、y、z轴的平移)和三个旋转自由度(绕x、y、z轴的旋转)，因此，目标六自由度位姿估计的目标是确定物体的位置和朝向，在计算机视觉、机器人学、虚拟现实等领域，六自由度位姿估计是一个关键问题，涉及到从传感器数据或图像中获取目标在空间中的准确位置和朝向。
几何特征：是指在几何学上描述物体或图像形状和结构的可测量属性，提供了有关物体或场景的几何信息，例如面积、周长、中心位置直径等。
基于单目摄像机和基本几何特征的姿态估计与跟踪算法已经被证明可行并且得到大量研究，双目摄像机与单目摄像机的差别在于，为了获得没有歧义的位姿估计，基于单目相机的位姿估计算法需要更多的几何特征，倘若目标上几何特征不足，则需要从不同角度拍摄目标的两幅以上图像，相比之下，基于双目相机的位姿估计算法由于从立体图像对中获取目标的基本几何特征的图像信息，因此需要的几何特征较少，而且鲁棒性较好。
Hu提出一种基于一个圆和两条直线的立体视觉位姿估计算法，该方法通过立体相机重建空间圆和空间直线段，通过空间直线段的方向和空间圆的旋转轴方向估计位姿；Jian提出一种基于一个圆和四个特征点的立体视觉位姿估计算法，该方法通过立体相机重建空间圆，并结合与圆共面的四个特征点，重建旋转矩阵，最后用正交投影迭代优化算法估计出目标的位姿；上述两种方法的缺点在于限制了基本几何特征的数量，并且基本几何特征之间需 要一些位置关系，例如直线必须互相平行等，均存在限制条件，难以推广。
由上可知，相关技术中仍存在准确率较低、泛用性和鲁棒性差的缺陷。
为此，本申请提供的基于双目视觉与粒子群优化的目标六自由度位姿估计方法，能够有效地提升基于双目视觉与粒子群优化的目标六自由度位姿估计的准确率、泛用性和鲁棒性，相应地，该基于双目视觉与粒子群优化的目标六自由度位姿估计方法适用于基于双目视觉与粒子群优化的目标六自由度位姿估计装置，该基于双目视觉与粒子群优化的目标六自由度位姿估计装置可部署于配置冯诺依曼体系结构的电子设备，例如，该电子设备可以是台式电脑、笔记本电脑、服务器等等。
为使本申请的目的、技术方案和优点更加清楚，下面将结合附图对本申请实施方式作进一步地详细描述。
图1为一种基于双目视觉与粒子群优化的目标六自由度位姿估计方法所涉及的实施环境的示意图。该实施环境包括采集端110和服务器端130。
具体地，采集端110获取目标的双目图像，该采集端110可以是任意具有双目图像采集功能的电子设备，在此不进行限定。
采集端110与服务器端130之间可以通过有线或者无线等方式建立的通信连接，以实现二者之间的数据传输。例如，传输的数据可以是双目图像等。
其中，服务器端130，也可以认为是云端、云平台、平台端、服务端等等，此服务器端130可以是一台服务器，也可以是由多台服务器构成的一个服务器集群，或者是由多台服务器构成的云计算中心，以便于更好地向海量采集端110提供后台服务。例如，后台服务包括基于双目视觉与粒子群优化的目标六自由度位姿估计服务。
随着采集端110与服务器端130的交互，在一个应用场景中，以服务器端130提供基于双目视觉与粒子群优化的目标六自由度位姿估计服务为例，采集端110获取到目标的双目图像后，将双目图像中包括的目标的旋转矩阵、位移向量和几何特征发送至服务器端130，那么，服务器端130便能够接收到该采集端110发送的目标的旋转矩阵、位移向量和几何特征，进而基于该目标的旋转矩阵、位移向量和几何特征提供基于双目视觉与粒子群优化的目 标六自由度位姿估计服务。具体而言，服务器端130获取到目标的旋转矩阵、位移向量和几何特征后，便通过旋转矩阵分解方法将旋转矩阵分解为旋转向量和旋转角度，再对旋转向量和旋转角度进行六自由度的均匀采样，得到数据点集，确保全方位的姿态变化被覆盖，再根据几何特征的几何性质和数据点集，建立旋转矩阵与位移向量之间的数学关系，最后通过设计目标函数和应用优化算法对目标的双目图像进行目标位姿估计，得到位姿估计结果。
请参阅图2，本申请实施例提供了一种基于双目视觉与粒子群优化的目标六自由度位姿估计方法，该方法适用于电子设备，该电子设备可以是图1所示出实施环境中的服务器端130，还可以是台式电脑、笔记本电脑、服务器等等。
在下述方法实施例中，为了便于描述，以该方法各步骤的执行主体为电子设备为例进行说明，但是并非对此构成具体限定。
如图2所示，该方法可以包括以下步骤：
步骤210，获取目标的双目图像。
其中，双目图像是通过两个摄像头(左摄像机和右摄像机)同时捕捉同一场景或对象的图像，这种摄像头布置模拟了人类的双眼视觉系统，能够提供深度信息，每个摄像头捕捉到的图像被称为双目立体图像对，其中一张图像对应于左眼视角，另一张图像对应于右眼视角，这两个图像之间的差异，也称为视差(disparity)，是在深度感知中非常重要的信息，通过计算视差，可以得出场景中不同点的深度或距离。
在一种可能的实现方式，双目图像中至少包括目标的旋转矩阵、位移向量和几何特征。
在一种可能的实现方式，获取目标的双目图像的可以通过首先选择和安装双目摄像头，然后进行双目摄像头的校准，确保两个摄像头之间的参数和位置准确，再使用相应的软件或编程语言(如Python中的OpenCV库)来捕获双目摄像头的图像，确保同步捕获两个摄像头的图像，以便后续的深度计算，使用立体匹配算法来计算深度信息，根据立体匹配的结果，生成深度图。
步骤230，通过旋转矩阵分解方法将旋转矩阵分解为旋转向量和旋转角 度，对旋转向量和旋转角度进行六自由度的均匀采样，得到数据点集。
在一种可能的实现方式，通过罗德里格斯公式将旋转矩阵分解为旋转向量和旋转角度。
具体的计算公式如下：
其中，参数左上角符号表示坐标名称，例如l、r、o分别表示左摄像机坐标系、右摄像机坐标系、目标坐标系；普通字体表示标量；粗体字表示矩阵或向量；R和t分别表示旋转矩阵和位移向量；I表示三维单位矩阵。
具体地，旋转向量n的计算公式如下：

在一种可能的实现方式，通过汤姆逊方法在单位球面对旋转向量进行六自由度的均匀采样，对0到π之间的旋转角度进行均匀采样，得到数据点集，确保数据点集中包括目标在六个自由度上的姿态变化，其中，六个自由度包括三个平移自由度和三个旋转自由度。
通过此种方式，本申请实施例通过汤姆逊方法在单位球面上对旋转向量进行均匀采样，汤姆逊方法旨在最小化系统的电势能，采样的旋转向量在球面上更加均匀，避免了其他非均匀采样方法可能引入的偏差，实现了更加稳定和有序的点分布，汤姆逊方法背后的物理原理基于库仑定律，模拟点之间的电荷相互作用，这种基于物理原理的方法更容易理解和解释，还可以通过调整库仑定律中的常数和初始配置优化过程，具有很强的可调性。
在一种可能的实现方式，通过上述过程实现目标的运动表示之后，根据左摄像机和右摄像机为目标构建立体相机模型。
具体地，如图4所示，以左摄像机和右摄像机的光心为原点，光轴为轴，建立lO-lXlYlZ和rO-rXrYrZ坐标系，图像平面在lZ＝fl和rZ＝fr处，其中fl和fr分别表示左摄像机和右摄像机的焦距，图像平面坐标系设成 oimgl-lulv和oimgr-rurv，其中lu、lv、ru、rv分别与lX、lY、rX、rY平行。
其中，向量符号上面加一横表示这个向量代表的基本几何特征在图像中的投影，例如：表示op在左图像的投影，在分别给定左摄像机和右摄像机的内参数矩阵和条件下，和与op的具体关系如下：

其中，lK12和rK12分别表示lK和rK的前两行所构成的矩阵，lK3和rK3分别表示lK和rK的第三行所构成的矩阵；lRo和lto表示目标坐标系相对左摄像机坐标的旋转矩阵和位移向量，rRo和rto表示目标坐标系相对左摄像机坐标的旋转矩阵和位移向量。在给定两个摄像机的旋转矩阵rRl和位移向量rtl的条件下，rRo和rto的具体表示方式如下：
rRo＝rRl lRo，
rto＝rRl lto+rtl.
通过上述过程，本申请实施例构建了目标的立体相机模型，通过模拟人类双眼视觉系统，获取了场景中目标的深度信息，可以推断目标距离相机的远近，提供了更为精确的物体定位信息，能够弥补单目相机在缺乏深度信息的情况下可能无法准确估计物体的距离这一缺陷，提高了深度感知的准确性，对于光照变化和纹理缺乏的环境更为鲁棒。
步骤250，根据几何特征的几何性质和数据点集，建立旋转矩阵与位移向量之间的数学关系。
具体地，步骤250可以包括以下步骤：
步骤251通过采样桑普森距离计算目标在所述双目图像上的投影点到数据点集的几何距离。
其中，几何距离为几何特征的几何性质，几何特征包括点、直线和面。
步骤253，根据投影点到数据点集的几何距离建立旋转矩阵与位移向量之间的数学关系。
步骤270，通过设计目标函数和应用优化算法，对目标的双目图像进行目标位姿估计，得到位姿估计结果。
其中，目标函数用于估计目标的姿态，优化算法用于优化位姿估计结果，位姿估计结果用于展示目标的六自由度姿态以及确定目标的位置和朝向。
在一种可能的实现方式，优化算法包括粒子群优化算法。
具体地，如图3所示，步骤270可以包括以下步骤：
步骤310，基于双目图像在立体视觉中的几何关系和旋转姿态设计目标函数。
在一种可能的实现方式，设计目标函数通过首先将目标的旋转矩阵参数化，得到目标的直线和圆的几何特征在双目图像中的几何距离，然后根据几何距离得到对应的桑普森距离，最后根据桑普森距离构建目标函数。
具体地，将旋转矩阵R参数化后，记作Ω＝[α β θ]T，设计目标函数如下：
其中，di表示数据点到线段的几何距离，si表示数据点到椭圆的桑普森距离，NL和NL分别表示图像上直线的数据点数量和椭圆的数据点数量；di的计算方式如下：
其中，qi表示第i个直线数据点，表示该线段上距离qi最近的点，的计算方法如下：

λ＝Median(0 λ* 1)，
其中，和分别表示模型直线在图像上的像，MedianO表示取中位数，如图5和图6所示，引入λ和λ*是为了避免数据点落在模型直线段的像所在的直线上，因为准确的位姿估计应该使数据点落在模型直线段的像上。
在一种可能的实现方式，目标函数中的si(Ω)表示数据点到椭圆的桑普森距离，具体计算公式如下：
其中，表示图像中圆的数据点qi的齐次坐标形式；和分别表示的第一、第二个特征；C′表示空间圆根据姿态计算得到的在图像上像的矩阵形式，具体计算公式如下：
C′＝κK-T((δI-ηCT)(δI-CηT)-r2ηηT)K-1，
其中，κ表示一个非零常数；K表示摄像机内参数矩阵(在本申请实施例中表示lK或rK)；δ表示光心到圆的支撑平面的距离；η表示空间圆的支撑平面的单位法向量；C表示空间圆在摄像机坐标系中的坐标；r表示空间圆的半径。
通过上述过程，本申请实施例得到了有利于获取目标准确位姿的目标函数，通过di(Ω)表示数据点到空间直线段在图像中的像的距离，实现了尽可能地保证模型的像被数据点包围，通过si(Ω)表示桑普森距离，比直接求解几何距离更加高效准确。
步骤330，通过粒子群优化算法在旋转矩阵与位移向量之间的参数空间中寻找最优的旋转矩阵和位移向量，根据寻找到的最优的旋转矩阵和位移向量对目标函数进行最小化。
步骤350，根据粒子群优化算法最小化后的目标函数估计目标相对于目标坐标系的相对位姿。
其中，目标坐标系用于量化目标的位姿变化。
具体地，粒子群优化算法(Particle Swarm Optimization，PSO)将每个优化问题的解抽象成搜索空间中一个没有质量和体积的微粒，所有粒子都有一个由目标函数决定的适应值、一个决定粒子移动方向和距离的速度，粒子们知道自己到目前为止发现的最好位置和现在的位置，此外每个粒子还知道到目前为止整个群体中所有粒子发现的最好位置，粒子就是通过自己发现的最好位置和群体发现的最好位置来决定自己的移动速度，最终找到全局最优解。
本申请实施例通过PSO对旋转矩阵R与平移向量t进行估计，旋转矩阵的自由度和平移向量的自由度决定了粒子群搜索空间的维度d＝6，现假设d维搜索空间中第i个粒子的位置和速度分别为Xi＝[xi1 xi2 ... xid]和 Vi＝[vi1 vi2 ... vid]，所有粒子的位置X和速度V的初始值随机生成。在每次迭代中，粒子通过跟踪两个最优解来更新自己，其一是粒子本身找到的最优解，记作Pbesti＝[pbesti1 pbesti2 ... pbestid]T；其二是整个群体目前找到的最优解，记作Gbestg＝[gbest1 gbest2 ... gbestd]T，粒子根据如下公式更新自己的速度和位置：

其中，j＝1，2，...，d，k表示当前迭代次数，w表示惯性权重，c1和c2表示正的学习因子，r1和r2为0到1之间的均匀分布的随机数。
本申请实施例所构建的t关于R的函数关系的物理意义是：将CAD模型根据姿态(R和t)投影到图像上，并使几何特征尽可能靠近其图像上所对应的数据点；直线特征方面，直线特征根据姿态的投影应该尽可能落在对应数据点(像素点)集内，如图6所示，如果用投影点到数据点集的拟合直线的几何距离作为该函数关系，则可能出现投影点落在数据点集区域以外，但仍在该点集的拟合直线上的情况，因此，如图5所示，将投影点到数据点集的拟合直线段的距离作为该函数关系，能够保证投影点尽可能落在数据点集区域以内；圆特征方面，如图7所示，本申请实施例采样桑普森距离，原因在于直接计算投影点到数据点集的拟合曲线的几何距离需要大量时间，会导致效率大幅降低，而桑普森距离可以近似几何距离并且便于计算。
在一种可能的实现方式，根据粒子群优化算法最小化后的目标函数，得到位姿估计结果之后，通过列文伯格马夸特算法对位姿估计结果中目标的相对位姿进行优化。
具体地，在列文伯格马夸尔特算法优化过程中，第k次迭代中的更新公式如下：
其中，如图8所示，在每次迭代都需要计算几何特征在图像中距离数据点最近的像，直线的最近点可以直接计算得到，而由于计算空间圆上在图像 中距离数据点最近的像需要大量的计算，所以本申请实施例引进中间变量以近距离点取代最近点，从而避免每次迭代都计算空间圆上距离数据点最近的点，通过本申请实施例设计的更新公式，目标函数收敛时对应的位姿(R和t)即为最后的目标位姿估计结果。
通过上述过程，本申请实施例通过首先获取获取目标的双目图像，双目图像中至少包括目标的旋转矩阵、位移向量和几何特征，通过旋转矩阵分解方法将旋转矩阵分解为旋转向量和旋转角度，然后对旋转向量和旋转角度进行六自由度的均匀采样，得到数据点集，确保全方位的姿态变化被覆盖，再根据几何特征的几何性质和数据点集，建立旋转矩阵与位移向量之间的数学关系，最后通过设计目标函数和应用优化算法对目标的双目图像进行目标位姿估计，得到位姿估计结果，通过粒子群优化算法及非线性优化算法，可以同时处理点、直线、圆三种基本几何特征，并且特征数量与相互间位置的关系没有限制，具有相当高的准确性、鲁棒性，进而解决了现有技术中存在的准确率较低、泛用性和鲁棒性差的问题。
图9展示了一示例性实施例与一现有技术的对比试验结果折线图。
具体地，如图9-1所示，本申请实施例与一现有技术进行了距离测试的对比实验，实验结果表明本申请实施例(pso线和pso+lm线)在旋转矩阵和位移向量的误差上均明显比该现有技术更好、更稳定、更鲁棒。
如图9-2所示，本申请实施例与一现有技术进行了噪声测试的对比实验，实验结果表明本申请实施例(pso线和pso+lm线)在旋转矩阵估计方面，明显优于该现有技术，且在位移向量估计方面，噪声增大至7以后，比该现有技术误差更小、鲁棒性明显增强。
如图9-3所示，本申请实施例与一现有技术进行了遮挡率测试的对比实验，实验结果表明本申请实施例(pso线和pso+lm线)在旋转矩阵方面，明显比该现有技术更准确、更稳定、更鲁棒，且在位移向量方面，当遮挡率小于0.2时，估计误差略大于该现有技术，当遮挡率大于0.2时，估计误差明显优于该现有技术。
图10展示了一示例性实施例与另一现有技术的对比试验结果折线图。
具体地，如图10-1所示，本申请实施例与一现有技术进行了距离测试的 对比实验，实验结果表明本申请实施例(pso线和pso+lm线)在位移向量估计误差相近，而在旋转矩阵估计方面明显比该现有技术更准确、更稳定、更鲁棒。
如图10-2所示，本申请实施例与一现有技术进行了噪声测试的对比实验，实验结果表明本申请实施例(pso线和pso+lm线)在位移向量方面当噪声小于8时，误差略大于该现有技术，当噪声大于8时，误差明显优于该现有技术；而在旋转矩阵估计方面明显比该现有技术更准确、更稳定、更鲁棒。
如图10-3所示，本申请实施例与一现有技术进行了遮挡率测试的对比实验，实验结果表明本申请实施例(pso线和pso+lm线)的位移向量估计方面，在遮挡率小时(遮挡率小于0.3)，误差略大于该现有技术，但遮挡率大时(遮挡率大于0.3)，误差明显优于该现有技术；而在旋转矩阵估计方面明显比该现有技术误差更小、更稳定、更鲁棒。
图11展示了一示例性实施例的直线特征和圆特征数量实验结果折线图。
具体地，如图11-1所示，本申请实施例进行了直线特征数量实验，可以看出随着直线特征数量增加，旋转矩阵和位移向量的误差均减小。
如图11-2所示，本申请实施例进行了圆特征数量实验，可以看出随着圆特征数量增加，旋转矩阵和位移向量的误差均减小。
通过上述过程，本申请实施例提高了旋转矩阵和位移向量的准确度，提高了位移向量估计的准确度，可以同时处理点、直线、圆三种基本几何特征，并且特征数量与相互间位置的关系没有限制，具有相当高的准确性、鲁棒性，进而解决了现有技术中存在的准确率较低、泛用性和鲁棒性差的问题。
图13是一应用场景中一种基于双目视觉与粒子群优化的目标六自由度位姿估计方法在模型1、模型2和模型3中的实现结果示意图。
具体地，如图13-1所示，本申请实施例在模型1上进行了实物实验，图(a)为左摄像机图像，图(b)为右摄像机图像，圆形线条是模型1根据本申请实施例估计的姿态投影到各个图像中的像；如图13-2所示，本申请实施例在模型2上进行了实物实验，图(a)为左摄像机图像，图(b)为右摄像机图像，圆形线条是模型2根据本申请实施例估计的姿态投影到各个图像中的像；如图13-3所示，本申请实施例在模型3上进行了实物实验，图(a) 为左摄像机图像，图(b)为右摄像机图像，圆形线条是模型3根据本申请实施例估计的姿态投影到各个图像中的像，从各图中均可看出模型的的像跟实物已经基本重合，说明本申请提供的方法估计出来的姿态非常接近真实姿态。
下述为本申请装置实施例，可以用于执行本申请所涉及的基于双目视觉与粒子群优化的目标六自由度位姿估计方法。对于本申请装置实施例中未披露的细节，请参照本申请所涉及的基于双目视觉与粒子群优化的目标六自由度位姿估计方法的方法实施例。
请参阅图13，本申请实施例中提供了一种基于双目视觉与粒子群优化的目标六自由度位姿估计装置800，包括但不限于：数据获取模块810、均匀采样模块830、数学关系构建模块850以及位姿估计模块870。
其中，数据获取模块810，用于获取目标的双目图像；双目图像中至少包括目标的旋转矩阵、位移向量和几何特征。
均匀采样模块830，用于通过旋转矩阵分解方法将旋转矩阵分解为旋转向量和旋转角度，对旋转向量和旋转角度进行六自由度的均匀采样，得到数据点集。
数学关系构建模块850，用于根据几何特征的几何性质和数据点集，建立旋转矩阵与位移向量之间的数学关系。
位姿估计模块870，用于通过设计目标函数和应用优化算法，对目标的双目图像进行目标位姿估计，得到位姿估计结果。
需要说明的是，上述实施例所提供的基于双目视觉与粒子群优化的目标六自由度位姿估计装置在进行基于双目视觉与粒子群优化的目标六自由度位姿估计时，仅以上述各功能模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能模块完成，即基于双目视觉与粒子群优化的目标六自由度位姿估计装置的内部结构将划分为不同的功能模块，以完成以上描述的全部或者部分功能。
另外，上述实施例所提供的基于双目视觉与粒子群优化的目标六自由度位姿估计装置与基于双目视觉与粒子群优化的目标六自由度位姿估计方法的实施例属于同一构思，其中各模块执行操作的具体方式已经在方法实施例中进行了详细描述，此处不再赘述。
图14根据一示例性实施例示出的一种服务器的结构示意。该服务器适用于图1所示出实施环境中的服务器端130。
需要说明的是，该服务器只是一个适配于本申请的示例，不能认为是提供了对本申请的使用范围的任何限制。该服务器也不能解释为需要依赖于或者必须具有图14示出的示例性的服务器2000中的一个或者多个组件。
服务器2000的硬件结构可因配置或者性能的不同而产生较大的差异，如图14所示，服务器2000包括：电源210、接口230、至少一存储器250、以及至少一中央处理器(CPU，Central Processing Units)270。
具体地，电源210用于为服务器2000上的各硬件设备提供工作电压。
接口230包括至少一有线或无线网络接口，用于与外部设备交互。例如，进行图1所示出实施环境中终端100与服务器200之间的交互。
当然，在其余本申请适配的示例中，接口230还可以进一步包括至少一串并转换接口233、至少一输入输出接口235以及至少一USB接口237等，如图14所示，在此并非对此构成具体限定。
存储器250作为资源存储的载体，可以是只读存储器、随机存储器、磁盘或者光盘等，其上所存储的资源包括操作系统251、应用程序253及数据255等，存储方式可以是短暂存储或者永久存储。
其中，操作系统251用于管理与控制服务器200上的各硬件设备以及应用程序253，以实现中央处理器270对存储器250中海量数据255的运算与处理，其可以是Windows ServerTM、Mac OS XTM、UnixTM、LinuxTM、FreeBSDTM等。
应用程序253是基于操作系统251之上完成至少一项特定工作的计算机程序，其可以包括至少一模块(图14未示出)，每个模块都可以分别包含有对服务器2000的计算机程序。例如，基于双目视觉与粒子群优化的目标六自由度位姿估计装置可视为部署于服务器2000的应用程序253。
数据255可以是存储于双目相机中的双目图像等，存储于存储器250中。
中央处理器270可以包括一个或多个以上的处理器，并设置为通过至少一通信总线与存储器250通信，以读取存储器250中存储的计算机程序，进而实现对存储器250中海量数据255的运算与处理。例如，通过中央处理器270读取存储器250中存储的一系列计算机程序的形式来完成基于双目视觉 与粒子群优化的目标六自由度位姿估计方法。
此外，通过硬件电路或者硬件电路结合软件也能同样实现本申请，因此，实现本申请并不限于任何特定硬件电路、软件以及两者的组合。
请参阅图15，本申请实施例中提供了一种电子设备4000，该电子设备400可以包括：(需要根据本申请具体情况适应性地修改)台式电脑、笔记本电脑、服务器等。
在图15中，该电子设备4000包括至少一个处理器4001、至少一条通信总线4002以及至少一个存储器4003。
其中，处理器4001和存储器4003相连，如通过通信总线4002相连。可选地，电子设备4000还可以包括收发器4004，收发器4004可以用于该电子设备与其他电子设备之间的数据交互，如数据的发送和/或数据的接收等。需要说明的是，实际应用中收发器4004不限于一个，该电子设备4000的结构并不构成对本申请实施例的限定。
处理器4001可以是CPU(Central Processing Unit，中央处理器)，通用处理器，DSP(Digital Signal Processor，数据信号处理器)，ASIC(Application Specific Integrated Circuit，专用集成电路)，FPGA(Field Programmable Gate Array，现场可编程门阵列)或者其他可编程逻辑器件、晶体管逻辑器件、硬件部件或者其任意组合。其可以实现或执行结合本申请公开内容所描述的各种示例性的逻辑方框，模块和电路。处理器4001也可以是实现计算功能的组合，例如包含一个或多个微处理器组合，DSP和微处理器的组合等。
通信总线4002可包括一通路，在上述组件之间传送信息。通信总线4002可以是PCI(Peripheral Component Interconnect，外设部件互连标准)总线或EISA(Extended Industry Standard Architecture，扩展工业标准结构)总线等。通信总线4002可以分为地址总线、数据总线、控制总线等。为便于表示，图10中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。
存储器4003可以是ROM(Read Only Memory，只读存储器)或可存储静态信息和指令的其他类型的静态存储设备，RAM(Random Access Memory，随机存取存储器)或者可存储信息和指令的其他类型的动态存储设备，也可以是EEPROM(Electrically Erasable Programmable Read Only Memory，电可擦可编程只读存储器)、CD-ROM(Compact Disc Read Only Memory，只读光 盘)或其他光盘存储、光碟存储(包括压缩光碟、激光碟、光碟、数字通用光碟、蓝光光碟等)、磁盘存储介质或者其他磁存储设备、或者能够用于携带或存储具有指令或数据结构形式的期望的程序代码并能够由计算机存取的任何其他介质，但不限于此。
存储器4003上存储有计算机程序，处理器4001通过通信总线4002读取存储器4003中存储的计算机程序。
该计算机程序被处理器4001执行时实现上述各实施例中的基于双目视觉与粒子群优化的目标六自由度位姿估计方法。
此外，本申请实施例中提供了一种存储介质，该存储介质上存储有计算机程序，该计算机程序被处理器执行时实现上述各实施例中的基于双目视觉与粒子群优化的目标六自由度位姿估计方法。
本申请实施例中提供了一种计算机程序产品，该计算机程序产品包括计算机程序，该计算机程序存储在存储介质中。计算机设备的处理器从存储介质读取该计算机程序，处理器执行该计算机程序，使得该计算机设备执行上述各实施例中的基于双目视觉与粒子群优化的目标六自由度位姿估计方法。
与相关技术相比，本申请的有益效果是：
1.本申请提出了一种基于双目视觉与粒子群优化的目标六自由度位姿估计方法。本申请通过首先获取获取目标的双目图像，双目图像中至少包括目标的旋转矩阵、位移向量和几何特征，通过旋转矩阵分解方法将旋转矩阵分解为旋转向量和旋转角度，然后对旋转向量和旋转角度进行均匀采样，确保全方位的姿态变化被覆盖，再根据几何特征的几何性质，建立旋转矩阵与位移向量之间的数学关系，最后通过设计目标函数和应用优化算法进行目标位姿估计，通过粒子群优化算法及非线性优化算法，可以同时处理点、直线、圆三种基本几何特征，并且特征数量与相互间位置的关系没有限制，具有相当高的准确性、鲁棒性，进而解决了现有技术中存在的准确率较低、泛用性和鲁棒性差的问题。
2.本申请提出一种基于立体视觉和粒子群优化的目标六自由度位置与姿态(位姿)估计算法，该算法利用目标上的基本几何特征(圆、直线)，根据基本几何特征的投影方程，精确计算目标位姿，并且不受几何特征数量与相互间位置关系的限制，实验表明本申请估计的位姿非常精确，为下游任务例 如装配、对接提供了可靠的位姿信息。
3.本申请通过粒子群优化算法及非线性优化算法，可以同时处理点、直线、圆三种基本几何特征，并且特征数量与相互间位置的关系没有限制，具有相当高的准确性、鲁棒性。
4.本申请提出一种基于立体视觉的粒子群优化算法，实现物体相对位姿的估计，首先通过罗德里格斯公式将旋转矩阵分解旋转向量和旋转角度，并利用汤姆逊方法，在单位球面对旋转向量进行均匀采样，旋转角度在到π之间均匀采样，再通过立体相机中基本几何特征本身的几何特性，建立旋转矩阵与位移向量之间的数学关系，然后结合粒子群优化算法和我们自己设计目标函数，可以估计出物体的姿态，最后，用莱文伯格-马夸特方法对粒子群优化算法估计的位姿进行优化，得到高精度、高鲁棒性的位姿估计。
应该理解的是，虽然附图的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，其可以以其他的顺序执行。而且，附图的流程图中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，其执行顺序也不必然是依次进行，而是可以与其他步骤或者其他步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
上述内容，仅为本申请的较佳示例性实施例，并非用于限制本申请的实施方案，本领域普通技术人员根据本申请的主要构思和精神，可以十分方便地进行相应的变通或修改，故本申请的保护范围应以权利要求书所要求的保护范围为准。

Claims (10)

  1. 一种基于双目视觉与粒子群优化的目标六自由度位姿估计方法，其特征在于，所述方法包括：
    获取目标的双目图像；所述双目图像中至少包括目标的旋转矩阵、位移向量和几何特征；
    通过旋转矩阵分解方法将所述旋转矩阵分解为旋转向量和旋转角度，对所述旋转向量和旋转角度进行六自由度的均匀采样，得到数据点集；
    根据所述几何特征的几何性质和所述数据点集，建立所述旋转矩阵与位移向量之间的数学关系；
    通过设计目标函数和应用优化算法，对所述目标的双目图像进行目标位姿估计，得到位姿估计结果；所述目标函数用于估计目标的姿态；所述优化算法用于优化位姿估计结果；所述位姿估计结果用于展示目标的六自由度姿态以及确定目标的位置和朝向。
  2. 如权利要求1所述的方法，其特征在于，所述通过旋转矩阵分解方法将所述旋转矩阵分解为旋转向量和旋转角度，包括：
    通过罗德里格斯公式将旋转矩阵分解为旋转向量和旋转角度。
  3. 如权利要求1所述的方法，其特征在于，所述对所述旋转向量和旋转角度进行六自由度的均匀采样，得到数据点集，包括：
    通过汤姆逊方法在单位球面对旋转向量进行六自由度的均匀采样；
    对0到π之间的旋转角度进行均匀采样，得到数据点集，确保所述数据点集中包括目标在六个自由度上的姿态变化；所述六个自由度包括三个平移自由度和三个旋转自由度。
  4. 如权利要求1所述的方法，其特征在于，所述根据所述几何特征的几何性质和所述数据点集，建立所述旋转矩阵与位移向量之间的数学关系，包括：
    通过采样桑普森距离计算目标在所述双目图像上的投影点到数据点集的几何距离；所述几何距离为几何特征的几何性质；所述几何特征包括点、直线和面；
    根据所述投影点到数据点集的几何距离建立所述旋转矩阵与位移向量之 间的数学关系。
  5. 如权利要求1所述的方法，其特征在于，所述优化算法包括粒子群优化算法；
    所述通过设计目标函数和应用优化算法，对所述目标的双目图像进行目标位姿估计，得到位姿估计结果，包括：
    基于所述双目图像在立体视觉中的几何关系和旋转姿态设计目标函数；所述目标函数包含立体视觉中的几何关系和旋转姿态的约束条件；
    通过粒子群优化算法在所述旋转矩阵与位移向量之间的参数空间中寻找最优的旋转矩阵和位移向量，根据寻找到的最优的旋转矩阵和位移向量对所述目标函数进行最小化；
    根据所述粒子群优化算法最小化后的目标函数，估计目标相对于目标坐标系的相对位姿，得到所述位姿估计结果；所述目标坐标系用于量化目标的位姿变化。
  6. 如权利要求5所述的方法，其特征在于，所述设计目标函数，包括：
    将所述目标的旋转矩阵参数化，得到目标的直线和圆的几何特征在所述双目图像中的几何距离；
    根据所述几何距离得到对应的桑普森距离，根据所述桑普森距离构建目标函数。
  7. 如权利要求5所述的方法，其特征在于，所述得到所述位姿估计结果之后，所述方法还包括：
    通过列文伯格马夸特算法对所述位姿估计结果中所述目标的相对位姿进行优化。
  8. 一种基于双目视觉与粒子群优化的目标六自由度位姿估计装置，其特征在于，所述装置包括：
    数据获取模块，用于获取目标的双目图像；所述双目图像中至少包括目标的旋转矩阵、位移向量和几何特征；
    均匀采样模块，用于通过旋转矩阵分解方法将所述旋转矩阵分解为旋转向量和旋转角度，对所述旋转向量和旋转角度进行六自由度的均匀采样，得到数据点集；
    数学关系构建模块，用于根据所述几何特征的几何性质和所述数据点集， 建立所述旋转矩阵与位移向量之间的数学关系；
    位姿估计模块，用于通过设计目标函数和应用优化算法，对所述目标的双目图像进行目标位姿估计，得到位姿估计结果。
  9. 一种电子设备，其特征在于，包括：至少一个处理器以及至少一个存储器，其中，
    所述存储器上存储有程序指令或代码；
    所述程序指令或代码被所述处理器加载并执行，使得电子设备实现如权利要求1至7中任一项所述的基于双目视觉与粒子群优化的目标六自由度位姿估计方法。
  10. 一种存储介质，其上存储有程序指令或代码，其特征在于，所述程序指令或代码被处理器加载并执行，以实现如权利要求1至7中任一项所述的基于双目视觉与粒子群优化的目标六自由度位姿估计方法。
PCT/CN2023/139298 2023-12-15 2023-12-15 基于双目视觉与粒子群优化的目标六自由度位姿估计方法 Pending WO2025123370A1 (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
PCT/CN2023/139298 WO2025123370A1 (zh) 2023-12-15 2023-12-15 基于双目视觉与粒子群优化的目标六自由度位姿估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/CN2023/139298 WO2025123370A1 (zh) 2023-12-15 2023-12-15 基于双目视觉与粒子群优化的目标六自由度位姿估计方法

Publications (1)

Publication Number Publication Date
WO2025123370A1 true WO2025123370A1 (zh) 2025-06-19

Family

ID=96056317

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2023/139298 Pending WO2025123370A1 (zh) 2023-12-15 2023-12-15 基于双目视觉与粒子群优化的目标六自由度位姿估计方法

Country Status (1)

Country Link
WO (1) WO2025123370A1 (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN121560082A (zh) * 2026-01-22 2026-02-24 昆山康泰达智能科技有限公司 一种用于xr设备双目主动对准设备的双目相机调位方法
CN121599970A (zh) * 2026-01-28 2026-03-03 杭州慧景科技股份有限公司 基于视觉位姿估计的道岔工况状态估计方法及系统

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101419055A (zh) * 2008-10-30 2009-04-29 北京航空航天大学 基于视觉的空间目标位姿测量装置和方法
CN108053443A (zh) * 2017-11-20 2018-05-18 中国科学院空间应用工程与技术中心 一种基于粒子群优化的物体点云位姿估算方法和系统
US20190204084A1 (en) * 2017-09-29 2019-07-04 Goertek Inc. Binocular vision localization method, device and system
CN114952832A (zh) * 2022-05-13 2022-08-30 清华大学 基于单目六自由度物体姿态估计的机械臂拼装方法及装置
CN115187781A (zh) * 2022-07-12 2022-10-14 北京信息科技大学 一种基于语义分割网络的六自由度抓取检测算法
CN116776719A (zh) * 2023-05-18 2023-09-19 哈尔滨工程大学 一种骨干粒子群优化算法的水下运载器机械手运动优化方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101419055A (zh) * 2008-10-30 2009-04-29 北京航空航天大学 基于视觉的空间目标位姿测量装置和方法
US20190204084A1 (en) * 2017-09-29 2019-07-04 Goertek Inc. Binocular vision localization method, device and system
CN108053443A (zh) * 2017-11-20 2018-05-18 中国科学院空间应用工程与技术中心 一种基于粒子群优化的物体点云位姿估算方法和系统
CN114952832A (zh) * 2022-05-13 2022-08-30 清华大学 基于单目六自由度物体姿态估计的机械臂拼装方法及装置
CN115187781A (zh) * 2022-07-12 2022-10-14 北京信息科技大学 一种基于语义分割网络的六自由度抓取检测算法
CN116776719A (zh) * 2023-05-18 2023-09-19 哈尔滨工程大学 一种骨干粒子群优化算法的水下运载器机械手运动优化方法

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN121560082A (zh) * 2026-01-22 2026-02-24 昆山康泰达智能科技有限公司 一种用于xr设备双目主动对准设备的双目相机调位方法
CN121560082B (zh) * 2026-01-22 2026-04-07 昆山康泰达智能科技有限公司 一种用于xr设备双目主动对准设备的双目相机调位方法
CN121599970A (zh) * 2026-01-28 2026-03-03 杭州慧景科技股份有限公司 基于视觉位姿估计的道岔工况状态估计方法及系统

Similar Documents

Publication Publication Date Title
JP7193494B2 (ja) オブジェクトの姿勢推定を生成するシステム、デバイス、および方法
CN110189399B (zh) 一种室内三维布局重建的方法及系统
Jing et al. Sampling-based view planning for 3d visual coverage task with unmanned aerial vehicle
Bazin et al. Rotation estimation and vanishing point extraction by omnidirectional vision in urban environment
CN107953329B (zh) 物体识别和姿态估计方法、装置及机械臂抓取系统
US11882262B2 (en) System and method for stereoscopic image analysis
WO2025123370A1 (zh) 基于双目视觉与粒子群优化的目标六自由度位姿估计方法
CN113361365B (zh) 定位方法和装置、设备及存储介质
CN102750704A (zh) 一种摄像机分步自标定方法
CN112712030B (zh) 三维姿态信息还原方法及装置
WO2023102552A1 (en) System and methods for validating imagery pipelines
CN118196186B (zh) 基于双目视觉与粒子群优化的目标六自由度位姿估计方法
Guan et al. Efficient recovery of multi-camera motion from two affine correspondences
CN107240149A (zh) 基于图像处理的物体三维模型构建方法
Lui et al. An Iterative 5-pt Algorithm for Fast and Robust Essential Matrix Estimation.
CN117830361B (zh) 一种点云配准方法、装置、计算机设备及存储介质
CN119832151A (zh) 开放式三维重建方法、自动深度定位方法、设备及机器人
CN113048985B (zh) 已知相对旋转角度条件下的像机相对运动估计方法
CN114137247A (zh) 基于事件相机的速度感知方法、装置、设备和介质
Liu et al. A Robot 3D Grasping Application Based on Binocular Vision System
CN112308911A (zh) 端到端的视觉定位方法及系统
CN117015966A (zh) 一种光场预测模型的生成方法及相关装置
CN117808979B (zh) 三维模型的构建方法及装置
CN120107475B (zh) 基于单目视觉的飞行目标轨道重建方法及系统
CN116310670B (zh) 基于曼哈顿世界结构空间融合的视觉slam方法及设备

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 23961226

Country of ref document: EP

Kind code of ref document: A1