A virtual multi-view six-degree-of-freedom pose measurement method, medium and system

Abstract

The present application belongs to the technical field of space geometry measurement, and particularly relates to a virtual multi-view six-degree-of-freedom pose measurement method, medium and system. The pose information of a cooperative target located in a combined field of view is obtained through coaxial cameras and a prism; a virtual camera is formed by rotating the prism; a coordinate system is established; target rotation representation is performed; a first rotation matrix is estimated; a virtual camera visual axis and a visual axis included angle are determined; a second rotation matrix is determined; a rotation error cost function is established; the rotation matrix and the rotation error cost function are represented by a quaternion; multi-rotation information fusion is performed; an exit point and an exit light vector of the prism are determined; feature point coordinates of the cooperative target are represented; determined object-side residuals are determined; an object-side residual cost function is constructed; and translation information fusion is performed. Compared with the prior art, the present application solves the problem that existing six-degree-of-freedom pose measurement devices are susceptible to noise, and the method proposed in the present scheme effectively improves measurement accuracy, adaptability and stability.

Description

Technical Field

[0001] This invention belongs to the field of spatial geometry measurement technology, specifically relating to a virtual multi-viewpoint six-degree-of-freedom pose measurement method, medium, and system. Background Technology

[0002] With the development of intelligent manufacturing, the extraction of motion condition information from manufacturing equipment is a crucial feedback link in intelligent manufacturing. As a key sensor in information extraction, the testing system directly determines the working accuracy of the manufacturing system. Currently, six-degree-of-freedom measurement devices used in industrial settings are typically difficult to operate, have complex overall components, occupy a large space, are not easily portable, and have high market prices, making them difficult to widely apply in industrial production.

[0003] Chinese invention patent CN102636139A proposes a spatial six-degree-of-freedom dynamic measurement device and method. The device uses multiple pull-wire displacement sensors to measure the six degrees of freedom of the target under test. However, the measurement device uses contact measurement, which limits its applicability to various measurement scenarios.

[0004] The prior art (G. Schweighofer and A. Pinz, “Globally optimal o(n) solution to the pnp problem for general camera models.” in BMVC, 2008, pp. 1-10; Zheng Y, Sugimoto S, Okutomi M. ASPnP: An Accurate and Scalable Solution to the Perspective-n-Point Problem[J]. Ice Transactions on Information & Systems, 2013, E96. D(7): 1525-1535.) proposed a six-degree-of-freedom visual measurement method, which can realize non-contact measurement of the target under test, but it is easily affected by noise such as light, vibration, and impact, and cannot meet the complex and ever-changing measurement needs of industrial sites.

[0005] Therefore, there is a need to propose a pose measurement system and method that can overcome the influence of noise from light, vibration, and impact on the measuring device. Summary of the Invention

[0006] The purpose of this invention is to provide a virtual multi-viewpoint six-degree-of-freedom pose measurement method, medium, and system to solve at least one of the above-mentioned problems, thereby addressing the issue that existing six-degree-of-freedom pose measurement devices are susceptible to noise from illumination, vibration, and impact. The virtual multi-viewpoint six-degree-of-freedom pose measurement method proposed in this solution effectively improves measurement accuracy, adaptability, and stability.

[0007] The objective of this invention is achieved through the following technical solution:

[0008] The first aspect of this invention discloses a virtual multi-viewpoint six-degree-of-freedom pose measurement method, which acquires the pose information of a cooperative target through a combination of a single camera and a prism; the camera and the prism are arranged coaxially, and the cooperative target is located within the combined field of view of the camera and the prism;

[0009] The method includes the following steps:

[0010] A virtual camera is created by rotating a prism;

[0011] S1: Establish coordinate system: Construct camera coordinate system O r -X r Y r Z r Virtual camera coordinate system O v -X v Y v Z v Prism coordinate system O p -X p Y p Z p and the coordinate system of the cooperative target O t -X t Y t Z t ;

[0012] S2: Target rotation representation: Represents the rotation matrix R of the cooperative target coordinate system relative to the camera coordinate system;

[0013] S3: Capture the cooperative target using the camera, and estimate the rotation matrix of the cooperative target coordinate system relative to the virtual camera coordinate system using a pose calculation algorithm.

[0014] S4: Determine the virtual camera's line of sight and the angle ψ between the camera's line of sight and the virtual camera's line of sight based on the prism's parameters and rotation angle.

[0015] S5: Determine the rotation matrix of the virtual camera coordinate system relative to the camera coordinate system based on the prism parameters, the prism rotation angle, and the included angle ψ.

[0016] S6: Establish the rotation error cost function;

[0017] S7: Quaternion representation of a rotation matrix;

[0018] S8: Quaternion cost function for rotation error;

[0019] S9: Multi-rotation information fusion to obtain the optimal rotation matrix R;

[0020] S10: Determine the exit point and the vector of the outgoing ray of the prism;

[0021] S11: Represents the coordinates of the feature point of the cooperative target in the prism coordinate system;

[0022] S12: Determine the object-space residual between the feature point and the outgoing ray;

[0023] S13: Construct the material-side residual cost function;

[0024] S14: Fusion of translation information to obtain the optimal translation matrix t.

[0025] Preferably, the cross-section of the prism is wedge-shaped.

[0026] Preferably, the parameters of the camera are matched with the parameters of the prism.

[0027] Preferably, the camera parameters and the prism parameters are matched using the following formula:

[0028]

[0029] In the formula, α is the prism wedge angle, and n is the prism refractive index. and These are half the horizontal and half the vertical physical dimensions of the camera's image plane, respectively; f is the camera's focal length; γ c1 This is the camera's field of view.

[0030] Preferably, the pose calculation algorithm includes LHM, EPnP, RPnP, DLS, OPnP, ASPnP, SDP, PPnP, and EPPnP.

[0031] Preferably, in step S6, the error of the rotation matrix is ​​characterized by the least 2 norm to establish the rotation error cost function:

[0032]

[0033] in, Let be the rotation matrix of the virtual camera coordinate system relative to the camera coordinate system from the i-th viewpoint. Let be the rotation matrix of the cooperative target coordinate system relative to the virtual camera coordinate system from the i-th viewpoint.

[0034] Preferably, in step S8, the quaternion cost function of the rotation error is as follows:

[0035]

[0036] In the formula, The quaternion of the rotation matrix, S is the quaternion form group of the rotation matrix R. Let be the rotation matrix of the virtual camera coordinate system relative to the camera coordinate system from the i-th viewpoint. Let be the rotation matrix of the cooperative target coordinate system relative to the virtual camera coordinate system from the i-th viewpoint.

[0037] Preferably, in step S10, the outgoing ray vector of the prism is determined according to the law of refraction, and the exit point of the prism is determined according to spatial geometric relationships.

[0038] Preferably, in step S14, the translation matrix t is a function of the rotation matrix R:

[0039]

[0040] In the formula, n is the refractive index of the prism, and L i,j Let P be the vector of the outgoing ray. w,i K is a feature point of the cooperative target. i,j The point of origin.

[0041] A second aspect of the present invention discloses a computer-readable storage medium comprising a stored computer program, wherein, when the computer program is executed, it controls the device on which the computer-readable storage medium is located to perform any of the methods described above.

[0042] The third aspect of this invention discloses a virtual multi-viewpoint six-DOF pose measurement system for acquiring pose information of a cooperative target. The system includes a camera, a prism, a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor. When the processor executes the computer program, it implements any of the methods described above. The prism has a wedge-shaped cross-section, the camera and the prism are arranged coaxially, and the cooperative target is located within the combined field of view of the camera and the prism.

[0043] Compared with the prior art, the present invention has the following beneficial effects:

[0044] (1) By fusing multi-viewpoint pose information, the influence of noise such as illumination and vibration on the measurement can be reduced, resulting in higher measurement accuracy and robustness.

[0045] (2) By combining a camera and a prism, a single camera can achieve multi-camera and multi-view observation and measurement, which has the advantages of compact structure, low cost, flexible movement and convenient control. Attached Figure Description

[0046] Figure 1 This is a schematic diagram of the virtual multi-viewpoint six-degree-of-freedom pose measurement method of the present invention;

[0047] Figure 2This is a schematic diagram illustrating the attitude calculation principle in the virtual multi-viewpoint six-degree-of-freedom pose measurement method of the present invention.

[0048] Figure 3 This is a schematic diagram illustrating the displacement calculation principle in the virtual multi-viewpoint six-degree-of-freedom pose measurement method of the present invention.

[0049] In the image: 1-Camera; 2-Prism; 3-Cooperative target; 4-Virtual camera. Detailed Implementation

[0050] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments. These embodiments are based on the technical solution of the present invention and provide detailed implementation methods and specific operating procedures. However, the scope of protection of the present invention is not limited to the following embodiments.

[0051] For any matters not covered below, existing technologies may be used.

[0052] Example

[0053] like Figure 1-3 As shown, this application proposes a virtual multi-viewpoint six-degree-of-freedom pose measurement system. The measurement system includes a camera 1, a prism 2, and a cooperative target 3. The visual axis of the camera 1 is arranged coaxially with that of the prism 2. The cooperative target 3 is set in the combined field of view of the camera 1 and the prism 2. In order to ensure that the camera 1 and the prism 2 can work normally to form an image after being combined, the parameters of the camera 1 and the prism 2 must be matched.

[0054] Preferably, the cross-section of the prism is wedge-shaped;

[0055] Preferably, the camera parameters and the prism parameters can be matched using the following formula:

[0056]

[0057] In the formula, α is the prism wedge angle, and n is the prism refractive index. and These are half the horizontal and half the vertical physical dimensions of the camera's image plane, respectively; f is the camera's focal length; γ c1 This is the camera's field of view.

[0058] Measurement methods, such as Figure 1-3 The steps are as follows:

[0059] In this method, the camera's line of sight is adjusted by rotating a prism to create a virtual camera with an equivalent observation effect;

[0060] S1. Coordinate System Establishment. Construct the camera 1 coordinate system O. r -X r Y r Z r Virtual camera 4-coordinate system O v -X vY v Z v Prism 2 coordinate system O p -X p Y p Z p and the cooperative target 3 coordinate system O t -X t Y t Z t ;

[0061] S2. Target Rotation Representation. At any viewpoint, the rotation matrix of the cooperative target coordinate system relative to the camera coordinate system is represented as:

[0062]

[0063] In the formula, Let be the rotation matrix of the virtual camera coordinate system relative to the camera coordinate system. This is the rotation matrix of the cooperative target coordinate system relative to the virtual camera coordinate system.

[0064] S3. The camera captures the cooperative target, and a pose calculation algorithm is used to estimate the rotation matrix of the cooperative target coordinate system relative to the virtual camera coordinate system.

[0065] S4. Virtual camera view axis L v Sure:

[0066]

[0067] in, θ is the angle of rotation of the prism.

[0068] Based on geometric relationships, the angle θ of the prism's rotation is equivalent to the virtual camera's line of sight L. v With camera line of sight L c The included angle ψ:

[0069]

[0070] Since the axis of rotation of the prism must be perpendicular to L v With L c Vector L of the plane m ,but:

[0071]

[0072] S5. Rotation matrix of virtual camera coordinate system relative to camera coordinate system Sure:

[0073]

[0074] In the formula, I is a third-order identity matrix, and A p and Bp They are respectively:

[0075]

[0076]

[0077] S6. Establishment of the rotation error cost function. The error of the rotation matrix is ​​characterized by the least 2 norm, i.e.:

[0078]

[0079] in, Let be the rotation matrix of the virtual camera coordinate system relative to the camera coordinate system from the i-th viewpoint. Let be the rotation matrix of the cooperative target coordinate system relative to the virtual camera coordinate system from the i-th viewpoint.

[0080] S7. Quaternion representation of rotation matrices. Any quaternion can be represented as: Rotation matrices are represented using quaternions:

[0081]

[0082] In the formula, I 3×3 It is a 3×3 identity matrix, ρ is [q0,q1,q2], and ρ× is an antisymmetric matrix:

[0083]

[0084] S8. Quaternion cost function for rotation error. Combining S3 and S4, further derivation yields:

[0085]

[0086] In the formula, S is the quaternion form group of the rotation matrix R.

[0087] S9. Multi-rotation information fusion. Further simplification and derivation show that the rotation error cost function can be expressed as:

[0088]

[0089] In the formula, B b The eigenvector corresponding to the largest eigenvalue is the optimal solution for multi-rotation information fusion.

[0090] S10. Determination of the exit point and exit ray vector in the prism coordinate system. Image ray vector of the j-th image point at the i-th viewpoint:

[0091]

[0092] Among them, (u i,j ,v i,j ) is the image coordinate of the j-th image point from the i-th viewpoint, and f is the camera focal length.

[0093] According to the law of refraction, the vector of the outgoing ray from the prism is:

[0094]

[0095] Where N1 and N2 are the normal vectors of the two planes of the prism, respectively.

[0096] Based on spatial geometric relationships, the exit point K i,j It can be represented as:

[0097]

[0098] Where D1 is the distance between the camera and the prism, D2 is the center thickness of the prism, and θ is the rotation angle of the prism.

[0099] S11. Cooperative feature point representation. Feature points P of the cooperative target. w,i The coordinates in the prism coordinate system can be expressed as:

[0100] p pc,i =RP w,i +tK i,j

[0101] In the formula, t is the translation matrix of the cooperative target coordinate system relative to the camera coordinate system, and P w,i These are the coordinates of the feature points in the cooperative target coordinate system.

[0102] S12. Determining the object-space residual. Due to the presence of noise, there is an object-space residual between the feature point and the outgoing ray. The object-space residual of the i-th feature point can be expressed as:

[0103] e i (R,t)=(IL i,j (RP) w,i +tK i,j )

[0104] In the formula, I is the identity matrix.

[0105] S13. Material Residual Cost Function. Considering the material residuals existing at all feature points, the material residual cost function can be expressed as:

[0106]

[0107] S14. Translation Information Fusion. Given the rotation matrix R, the translation matrix t has an optimal solution:

[0108]

[0109] T(R,t) is the transformation matrix between the cooperative target coordinate system and the camera coordinate system.

[0110] Preferred, The solution algorithm is any one of LHM, EPnP, RPnP, DLS, OPnP, ASPnP, SDP, PPnP and EPPnP.

[0111] The above description of the embodiments is provided to enable those skilled in the art to understand and use the invention. It will be apparent to those skilled in the art that various modifications can be made to these embodiments, and the general principles described herein can be applied to other embodiments without inventive effort. Therefore, the present invention is not limited to the above embodiments, and any improvements and modifications made by those skilled in the art based on the disclosure of the present invention without departing from the scope of the invention should be within the protection scope of the present invention.

Claims

1. A virtual multi-viewpoint six-DOF pose measurement method, characterized in that, The pose information of the cooperative target is obtained by combining a single camera and a prism; the camera and prism are arranged coaxially, and the cooperative target is located within the combined field of view of the camera and prism. The method includes the following steps: A virtual camera is created by rotating a prism; S1: Establish coordinate system: Construct camera coordinate system O r -X r Y r Z r Virtual camera coordinate system O v -X v Y v Z v Prism coordinate system O p -X p Y p Z p and the coordinate system of the cooperative target O t -X t Y t Z t ; S2: Target rotation representation: Represents the rotation matrix R of the cooperative target coordinate system relative to the camera coordinate system; S3: Capture the cooperative target using the camera, and estimate the rotation matrix of the cooperative target coordinate system relative to the virtual camera coordinate system using a pose calculation algorithm. ; S4: Determine the virtual camera's line of sight and the angle between the camera's line of sight and the virtual camera's line of sight based on the prism's parameters and rotation angle. ; S5: Based on prism parameters, prism rotation angle, and included angle. Determine the rotation matrix of the virtual camera coordinate system relative to the camera coordinate system. ; S6: Establishing the rotation error cost function: The error of the rotation matrix is ​​characterized by the least 2 norm to establish the rotation error cost function. ； in, Let be the rotation matrix of the virtual camera coordinate system relative to the camera coordinate system from the i-th viewpoint. Let be the rotation matrix of the cooperative target coordinate system relative to the virtual camera coordinate system from the i-th viewpoint; S7: Quaternion representation of the rotation matrix R; S8: Quaternion cost function for rotation error; S9: Multi-rotation information fusion to obtain the optimal rotation matrix R; S10: Determine the exit point and the vector of the outgoing ray of the prism; S11: Represents the coordinates of the feature point of the cooperative target in the prism coordinate system; S12: Determine the object-space residual between the feature point and the outgoing ray; S13: Construct the material-side residual cost function; S14: Fusion of translation information to obtain the optimal translation matrix t.

2. The virtual multi-viewpoint six-DOF pose measurement method according to claim 1, characterized in that, The parameters of the camera are matched with the parameters of the prism.

3. The virtual multi-viewpoint six-DOF pose measurement method according to claim 2, characterized in that, The camera parameters and the prism parameters are matched using the following formula: ； In the formula, α is the prism wedge angle, and n is the prism refractive index. and These represent half the horizontal and half the vertical physical dimensions of the camera's image plane, respectively, where f is the camera's focal length. This is the camera's field of view.

4. The virtual multi-viewpoint six-DOF pose measurement method according to claim 1, characterized in that, The pose calculation algorithms include LHM, EPnP, RPnP, DLS, OPnP, ASPnP, SDP, PPnP, and EPPnP.

5. The virtual multi-viewpoint six-DOF pose measurement method according to claim 1, characterized in that, In step S8, the quaternion cost function for the rotation error is shown in the following equation: ； In the formula, The quaternion of the rotation matrix, S is the quaternion form group of the rotation matrix R. Let be the rotation matrix of the virtual camera coordinate system relative to the camera coordinate system from the i-th viewpoint. Let be the rotation matrix of the cooperative target coordinate system relative to the virtual camera coordinate system from the i-th viewpoint.

6. The virtual multi-viewpoint six-DOF pose measurement method according to claim 1, characterized in that, In step S10, the outgoing ray vector of the prism is determined according to the law of refraction, and the exit point of the prism is determined according to the spatial geometric relationship.

7. The virtual multi-viewpoint six-DOF pose measurement method according to claim 1, characterized in that, In step S14, the translation matrix t is a function of the rotation matrix R: ； In the formula, n is the refractive index of the prism, and L i,j Let P be the vector of the outgoing ray. w,i K represents the coordinates of the feature point in the cooperative target coordinate system. i,j Let I be the exit point, and let I be the identity matrix.

8. A computer-readable storage medium, characterized in that, The computer-readable storage medium includes a stored computer program, wherein, when the computer program is executed, it controls the device on which the computer-readable storage medium is located to perform the method as described in any one of claims 1 to 7.

9. A virtual multi-viewpoint six-DOF pose measurement system for acquiring pose information of a cooperative target, characterized in that, The system includes a camera, a prism, a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor, wherein the processor, when executing the computer program, implements the method as described in any one of claims 1 to 7; the prism has a wedge-shaped cross-section, the camera and the prism are arranged coaxially, and the cooperative target is located within the combined field of view of the camera and the prism.

Images