A calibrator matching method and device

Abstract

The present application discloses a calibrator matching method and device, which acquire a first image and a second image collected by a binocular camera, and extract feature points in the two images for characterizing circular markers; triangulate and reconstruct a first target feature point in the first image and a second target feature point in the second image to obtain corresponding three-dimensional space points; screen matching feature points of the first image and the second image based on the three-dimensional space points, and determine the three-dimensional space points corresponding to the generated matching feature points as three-dimensional points; form a calibrator from the three-dimensional points based on a preset rule, and match the calibrator with a standard calibrator to obtain the pose information of the calibrator. It can be seen that the method and device eliminate mismatched feature points through triangulation reconstruction, avoid the problem of difficult feature point matching in the left and right view images, accurately construct three-dimensional points, achieve stable low-texture calibrator reconstruction, and then accurately determine the pose information of the calibrator.

Description

Technical Field

[0001] The present application relates to the technical field of surgical positioning, and particularly to a method and device for calibrator matching. Background Art

[0002] When using a robot for surgery, an optical tracking system is required to position and track the human body posture. Currently, optical tracking surgical robots widely use binocular infrared cameras to locate and calibrate optical markers on a calibrator. And to enable the surgical robot to perceive the pose information of the calibrator, it is necessary to collect the left and right view images of the calibrator through the binocular cameras, and obtain the pose information of the calibrator through image analysis, so as to realize the tracking and positioning of the surgical target.

[0003] The calibrator usually consists of several circular markers. When determining the pose information of the calibrator based on image analysis, first, the left and right view images of several circular markers are collected through the binocular cameras, and then the feature points used to characterize the circular markers are extracted from the left and right view images respectively. The feature points in the left view image are matched with the feature points in the right view image to obtain the matching feature points, and then the matching feature points are used to construct the three-dimensional points of each circular marker in the three-dimensional space represented by the binocular cameras. The pose information of the calibrator is determined based on the three-dimensional points of each circular marker.

[0004] However, since the binocular cameras are usually infrared cameras, the circular markers are relatively bright white areas in the captured images, and the background part is very dark black areas, resulting in limited texture information that can be extracted. Then, in low-texture images, it is difficult to match the feature points in the left and right view images collected by the binocular cameras, resulting in the inability to construct three-dimensional points, and thus the inability to accurately determine the pose information of the calibrator. Summary of the Invention

[0005] The present application provides a method and device for calibrator matching to solve the problem that the existing method cannot accurately determine the pose information of the calibrator.

[0006] In a first aspect, the present application provides a method for calibrator matching, including:

[0007] Obtain a first image and a second image collected by a binocular camera, where both the first image and the second image include images of several circular markers;

[0008] Extract the feature points in the first image and the feature points in the second image, where the feature points are used to characterize the circular markers;

[0009] Triangulate and reconstruct the first target feature points in the first image and the second target feature points in the second image to obtain corresponding three-dimensional space points;

[0010] Based on the three-dimensional space points, the matching feature points of the first image and the second image are screened out, and the three-dimensional space points corresponding to the generated matching feature points are determined as three-dimensional points. The matching feature points refer to the feature points representing the same circular marker in the first image and the second image;

[0011] Based on a preset rule, the three-dimensional points are combined into a calibrator, and the calibrator is matched with a standard calibrator to obtain the pose information of the calibrator.

[0012] In some embodiments of the present application, the screening of the matching feature points of the first image and the second image based on the three-dimensional space points includes:

[0013] The three-dimensional space points are back-projected into the first image to form first projection points, and the three-dimensional space points are back-projected into the second image to form second projection points;

[0014] If the first projection point matches the first target feature point, and the second projection point matches the second target feature point, then the first target feature point and the second target feature point are determined as matching feature points.

[0015] In some embodiments of the present application, the step of determining the first target feature point and the second target feature point as matching feature points if the first projection point matches the first target feature point and the second projection point matches the second target feature point includes:

[0016] Calculate the first reprojection error between the first projection point and the first target feature point, and calculate the second reprojection error between the second projection point and the second target feature point;

[0017] Compare the first reprojection error with an error threshold, and compare the second reprojection error with the error threshold;

[0018] If both the first reprojection error and the second reprojection error are less than or equal to the error threshold, then the first target feature point and the second target feature point corresponding to the three-dimensional space points are determined as matching feature points;

[0019] If any one of the first reprojection error and the second reprojection error is greater than the error threshold, then it is determined that the first target feature point and the second target feature point do not match.

[0020] In some embodiments of the present application, the method further includes: when the first target feature point and the second target feature point do not match, screening for matching feature points in the first image and the second image according to a preset matching principle, where the preset matching principle includes traversing each feature point in the first image and each feature point in the second image in sequence from left to right, so as to select, in the second image, a feature point that matches any feature point in the first image, and deleting all feature points in the first image and the second image that do not have a matching relationship.

[0021] In some embodiments of the present application, screening for matching feature points in the first image and the second image according to the preset matching principle includes:

[0022] When the first target feature point and the second target feature point do not match, selecting the next feature point in the second image and performing a matching verification with the first target feature point;

[0023] If no feature point matching the first target feature point can be traversed in the second image, then deleting the first target feature point and traversing again from the next feature point in the first image and a specified feature point in the second image, where the specified feature point in the second image refers to a feature point in the second image that does not have a matching relationship with the first target feature point;

[0024] During the process of traversing the feature points of the second image, if a second target feature point matching the first target feature point is obtained, then stopping the traversal of the second image and traversing from the next feature point that matches successfully in the first image and the next feature point that matches successfully in the second image;

[0025] After all feature points in the first image and all feature points in the second image have been traversed, stopping the matching verification process, determining the feature points with a matching relationship in the first image and the second image as matching feature points, and deleting all feature points in the first image and the second image that do not have a matching relationship.

[0026] In some embodiments of the present application, before triangulating and reconstructing the first target feature point in the first image and the second target feature point in the second image to obtain corresponding three-dimensional space points, the method further includes: performing an epipolar rectification process on the first image and the second image, where the epipolar rectification process is used to convert the positions of the feature points representing the same circular marker in the first image and the second image to the same row.

[0027] In some embodiments of the present application, before triangulating and reconstructing the first target feature point in the first image and the second target feature point in the second image to obtain corresponding three-dimensional space points, the method further includes:

[0028] After epipolar rectification processing, obtain the number of feature points on each row in the first image and the number of feature points on each row in the second image;

[0029] On the same row, if the number of corresponding feature points in the first image is different from the number of corresponding feature points in the second image, then based on the positional relationship between the feature points in the first image and the feature points in the second image on the same row, delete the redundant feature points generated by feature point comparison in the first image and the second image according to a preset comparison rule, so that the number of corresponding feature points in the first image is the same as the number of corresponding feature points in the second image on the same row, and perform the step of triangulating and reconstructing the first target feature points in the first image and the second target feature points in the second image to obtain the corresponding three-dimensional space points.

[0030] In some embodiments of the present application, the method of forming a calibrator from three-dimensional points based on a preset rule includes:

[0031] In three-dimensional space, connect any two of the three-dimensional points into line segments;

[0032] Calculate the line segment distance of each line segment, and calculate the angle between any two line segments;

[0033] Obtain the standard line segment distance and standard angle of the standard calibrator. If the line segment distance and angle formed by the specified three-dimensional point and the associated three-dimensional point simultaneously meet the standard line segment distance and standard angle conditions, then determine the specified three-dimensional point as the target three-dimensional point, where the associated three-dimensional point refers to the three-dimensional point connected to the specified three-dimensional point to form a line segment;

[0034] Based on the target three-dimensional point and the corresponding associated three-dimensional point, form a calibrator according to the standard calibrator constraint rule.

[0035] In some embodiments of the present application, the method of matching the calibrator with the standard calibrator to obtain the pose information of the calibrator includes: using the iterative closest point algorithm to perform registration calculation on the calibrator and the corresponding standard calibrator to obtain the pose information of the calibrator, where the pose information of the calibrator includes the translation value and rotation value of the calibrator.

[0036] In some embodiments of the present application, the method further includes:

[0037] When there are multiple calibrators corresponding to the standard calibrator, calculate the calibrator matching error corresponding to each calibrator based on the translation value and rotation value of each calibrator;

[0038] Screen out the target calibrator corresponding to the minimum calibrator matching error, and determine the pose information of the calibrator based on the translation value and rotation value of the target calibrator.

[0039] In a second aspect, the present application also provides a calibrator matching device, including:

[0040] A data acquisition module, configured to acquire a first image and a second image collected by a binocular camera, where images of several circular markers are included in both the first image and the second image;

[0041] A feature point extraction module, configured to extract feature points in the first image and feature points in the second image, where the feature points are used to represent circular markers;

[0042] A three-dimensional space point reconstruction module, configured to perform triangulation reconstruction on a first target feature point in the first image and a second target feature point in the second image to obtain corresponding three-dimensional space points;

[0043] A three-dimensional point determination module, configured to screen out matching feature points of the first image and the second image based on the three-dimensional space points, and determine the three-dimensional space points corresponding to the generated matching feature points as three-dimensional points, where the matching feature points refer to feature points representing the same circular marker in the first image and the second image;

[0044] A pose information determination module, configured to form a calibrator with the three-dimensional points based on a preset rule, and match the calibrator with a standard calibrator to obtain the pose information of the calibrator.

[0045] A calibrator matching method and device provided by an embodiment of the present application acquire a first image and a second image collected by a binocular camera, and extract feature points for representing circular markers in the two images; perform triangulation reconstruction on a first target feature point in the first image and a second target feature point in the second image to obtain corresponding three-dimensional space points; screen out matching feature points of the first image and the second image based on the three-dimensional space points, and determine the three-dimensional space points corresponding to the generated matching feature points as three-dimensional points; form a calibrator with the three-dimensional points based on a preset rule, and match the calibrator with a standard calibrator to obtain the pose information of the calibrator. It can be seen that this method and device eliminate mismatched feature points through triangulation reconstruction, avoid the problem of difficult matching of feature points in left and right view images, accurately construct three-dimensional points, realize stable reconstruction of a low-texture calibrator, and further accurately determine the pose information of the calibrator. Description of the Drawings

[0046] To more clearly illustrate the technical solutions of the present application, the drawings required for use in the embodiments will be briefly introduced below. Obviously, for those of ordinary skill in the art, other drawings can also be obtained based on these drawings without creative efforts.

[0047] Figure 1 It is a schematic structural diagram of a calibrator.

[0048] Figure 2 Flow chart of the calibrator matching method provided for the exemplary embodiments of the present application.

[0049] Figure 3 Schematic diagram of epipolar line matching in an ideal state provided for the exemplary embodiments of the present application.

[0050] Figure 4 Schematic diagram of epipolar line matching in a non-ideal state provided for the exemplary embodiments of the present application.

[0051] Figure 5 Flow chart of the method for screening and matching feature points provided for the exemplary embodiments of the present application.

[0052] Figure 6 Flow chart of the method for determining matching feature points provided for the exemplary embodiments of the present application.

[0053] Figure 7 Flow chart of the method for forming a calibrator from three-dimensional points provided for the exemplary embodiments of the present application.

[0054] Figure 8 Schematic diagram of forming a calibrator based on three-dimensional points provided for the exemplary embodiments of the present application.

[0055] Figure 9 Schematic diagram of a binocular camera for collecting images provided for the exemplary embodiments of the present application.

[0056] Figure 10 Schematic diagram of the inconsistent number of feature points in the left and right view images on the same row provided for the exemplary embodiments of the present application.

[0057] Figure 11 Structural block diagram of the calibrator matching device provided for the exemplary embodiments of the present application. Detailed implementation manners

[0058] The present invention will be further clarified below in conjunction with the accompanying drawings and specific embodiments.

[0059] In some embodiments, a calibrator for navigation surgery is installed at the end of the robotic arm of a surgical robot. When using the robot for surgery, the left and right view images of the calibrator are collected by a binocular infrared camera, and the pose information of the calibrator is obtained through image analysis, so that the optical tracking system of the surgical robot can position and track the human body pose, and realize the tracking and positioning of the surgical target.

[0060] Figure 1 It is a schematic diagram of the structure of the calibrator. The calibrator is usually composed of several circular markers. For example, as Figure 1As shown in the figure, the calibrator consists of 4 circular markers, and the relative positions among the 4 circular markers are determined. The left and right view images of the 4 circular markers are collected by a binocular infrared camera. When performing image analysis, the three-dimensional points of each circular marker in the three-dimensional space represented by the binocular infrared camera are determined respectively, and then the pose information of the calibrator is determined based on the three-dimensional points of the 4 circular markers.

[0061] To accurately determine the pose information of the calibrator, an embodiment of the present application provides a calibrator matching method. By using the epipolar line matching method and eliminating the mismatched feature points through the reprojection error, a stable low-texture calibrator reconstruction is realized, ensuring the accuracy of determining the pose information of the calibrator.

[0062] Figure 2 The flowchart of the calibrator matching method provided by the exemplary embodiment of the present application is as follows. Figure 2 As shown in the figure, the calibrator matching method provided by the embodiment of the present application includes:

[0063] S1. Obtain a first image and a second image collected by the binocular camera.

[0064] Among them, both the first image and the second image include images of several circular markers.

[0065] The binocular camera includes two cameras arranged side by side, and the lenses of the two cameras face the same direction. By using the binocular camera to collect the calibrator, a left view image and a right view image can be obtained, and each image includes images of several circular markers. Among them, the first image is one of the left view image and the right view image, and the second image is the other of the left view image and the right view image.

[0066] S2. Extract the feature points in the first image and the feature points in the second image.

[0067] Among them, the feature points are used to represent the circular markers.

[0068] Feature point extraction is performed on the first image and the second image respectively. At least one feature point can be extracted from the first image, and at least one feature point can be extracted from the second image. The feature points extracted from the two images include the feature points used to represent the circular markers.

[0069] In some embodiments, if there are also images of impurity targets in the image, for example, the imaging of the front part of the robotic arm located around the calibrator, other surgical facilities in the surgical environment, etc., then the feature points extracted from the two images also include the feature points representing the impurity targets. Among them, the impurity target refers to other objects except the circular markers within the acquisition range of the binocular camera.

[0070] S3. Triangulate and reconstruct the first target feature points in the first image and the second target feature points in the second image to obtain corresponding three-dimensional space points.

[0071] Among them, the parameters required for triangulation reconstruction may include the projection matrix and camera parameters of the binocular camera, etc. Camera parameters include, but are not limited to, camera internal parameters and camera external parameters. Camera internal parameters are parameters related to the characteristics of the camera itself, such as the focal length and pixel size of the camera; camera external parameters are parameters in the world coordinate system, such as the camera position and attitude, etc.

[0072] When constructing the three-dimensional points representing the circular marker, it is necessary to ensure that the feature points in the first image and the feature points in the second image are in a matching relationship. Therefore, match the feature points in the first image and the feature points in the second image to determine the matching feature points representing the same circular marker. When performing feature point matching, usually, the respective feature points in the two images can be matched in sequence from left to right.

[0073] Figure 3 This is a schematic diagram of epipolar line matching in the ideal state provided by an exemplary embodiment of the present application. As Figure 3 shown, in the ideal state, the order of the feature points from left to right in the left view image is the same as the order of the feature points from left to right in the right view image, indicating that there is a one-to-one correspondence between the feature points from left to right in the left and right view images, that is, the respective feature points in the left and right view images are matched.

[0074] However, since the binocular camera may collect other impurity targets around the calibrator when collecting the calibrator, and due to the perspective limitations of the left and right cameras, the feature points from left to right in the left and right view images may not be in a completely one-to-one correspondence.

[0075] Figure 4 This is a schematic diagram of epipolar line matching in the non-ideal state provided by an exemplary embodiment of the present application. As Figure 4 shown, the first and second feature points from the left in the left view image and the first and second feature points from the left in the right view image are not in a corresponding relationship, resulting in the non-matching of the first feature point from the left in the left view image and the first feature point from the left in the right view image, and, the non-matching of the second feature point from the left in the left view image and the second feature point from the left in the right view image, thereby leading to the occurrence of mis-matching during feature point matching.

[0076] To eliminate mis-matched feature points and screen out the matching feature points representing the circular marker, the embodiment of the present application uses the method of calculating the reprojection error. First, triangulation reconstruction is performed based on the first target feature points in the first image and the second target feature points in the second image to determine the three-dimensional space points. Then, based on the three-dimensional space points, the matching feature points between the first image and the second image are screened out, and the three-dimensional space points corresponding to the generated matching feature points are determined as three-dimensional points. Based on the screened three-dimensional points, the accuracy of the subsequent determination of the pose information of the calibrator can be ensured.

[0077] In some embodiments, based on the projection matrix and camera parameters of the binocular camera, each feature point of the first image in the two-dimensional space and each feature point in the second image are triangulated and reconstructed in sequence according to the corresponding relationship to obtain the corresponding three-dimensional space points. Ideally, each three-dimensional space point represents a circular marker. However, if there are impurity targets in the image, the objects represented by some three-dimensional space points may be impurity targets.

[0078] Among them, the origin of the two-dimensional space of the image can be located at the upper left corner of the image, the X-axis direction is from left to right, and the Y-axis direction is from top to bottom. The three-dimensional space of the binocular camera refers to the space where the camera three-dimensional coordinate system of the binocular camera is located. The origin of the camera coordinate system can be on any camera of the binocular camera. For example, taking the optical center of the left camera as the origin, the X-axis direction is the horizontal direction where the two cameras are located, that is, the connection direction of the two cameras, the Y-axis direction is the vertical direction, that is, the direction from the binocular camera to the ground, and the Z-axis direction is the direction from the binocular camera to the object to be photographed, that is, on the axis of the camera lens.

[0079] S4. Based on the three-dimensional space points, screen out the matching feature points between the first image and the second image, and determine the three-dimensional space points corresponding to the generated matching feature points as three-dimensional points.

[0080] The embodiment of the present application back-projects the three-dimensional space points into the first image and the second image respectively, and calculates the reprojection error formed in each image. Compare each reprojection error with the error threshold, and based on the comparison result, screen out the matching feature points between the first image and the second image. The matching feature points refer to the feature points representing the same circular marker in the first image and the second image.

[0081] Figure 5 This is the flowchart of the method for screening matching feature points provided by the exemplary embodiment of the present application. As Figure 5 shown, in some embodiments, when performing step S4, that is, when performing the process of screening out the matching feature points between the first image and the second image based on the three-dimensional space points, it includes:

[0082] S41. Back-project the three-dimensional space points into the first image to form the first projection points, and back-project the three-dimensional space points into the second image to form the second projection points.

[0083] S42. If the first projection point matches the first target feature point, and the second projection point matches the second target feature point, then determine the first target feature point and the second target feature point as matching feature points.

[0084] Exemplarily, triangulate and reconstruct the feature point A1 in the first image and the feature point B1 in the second image to form a three-dimensional space point C1. The three-dimensional space point C1 represents an object X, and the object X may be a circular marker or an impurity target. Back-project the three-dimensional space point C1 onto the first image to obtain a first projection point A1', and back-project the three-dimensional space point C1 onto the second image to obtain a second projection point B1'. Calculate a first reprojection error W1 between the feature point A1 in the first image and the first projection point A1', and calculate a second reprojection error W2 between the feature point B1 in the second image and the second projection point B1'.

[0085] In some embodiments, when calculating the reprojection error, it can be determined by calculating the pixel distance between the target feature point and the corresponding projection point, and the pixel distance between two points can be calculated according to the pixel coordinates of the two points in the two-dimensional image.

[0086] For example, when calculating the first reprojection error W1, in the first image in the two-dimensional space, obtain the pixel coordinates P1(x1, y1) of the feature point A1 and the pixel coordinates P1'(x1', y1') of the first projection point A1'. Based on P1(x1, y1) and P1'(x1', y1'), calculate the line segment distance H1 between the feature point A1 and the first projection point A1', and use the line segment distance H1 as the first reprojection error W1. It should be noted that the calculation method of the second reprojection error can refer to this example method and will not be elaborated here.

[0087] Judge the magnitude relationship between the reprojection error generated by the first target feature point and the second target feature point and the error threshold to screen out the matching feature points of the first image and the second image. If the reprojection error is greater than the error threshold, that is, the reprojection error is too large, it means that the first target feature point in the first image and the second target feature point in the second image do not match; if the reprojection error is less than or equal to the error threshold, it means that the first target feature point in the first image and the second target feature point in the second image match and are determined as matching feature points. In an ideal state, each matching feature point refers to the feature points in the first image and the second image that represent the same circular marker. However, if there are impurity targets in the image, some matching feature points may refer to the feature points in the first image and the second image that represent the same impurity target.

[0088] Figure 6The flowchart of the method for determining matching feature points provided by an exemplary embodiment of the present application. As Figure 6 shown, in some embodiments, when performing step S42, that is, when performing the process of determining the first target feature point and the second target feature point as matching feature points if the first projection point matches the first target feature point and the second projection point matches the second target feature point, it includes:

[0089] S421. Calculate the first reprojection error between the first projection point and the first target feature point, and calculate the second reprojection error between the second projection point and the second target feature point.

[0090] S422. Compare the first reprojection error with the error threshold, and compare the second reprojection error with the error threshold.

[0091] S423. If both the first reprojection error and the second reprojection error are less than or equal to the error threshold, determine the first target feature point and the second target feature point corresponding to the three-dimensional space point as matching feature points.

[0092] S424. If any one of the first reprojection error and the second reprojection error is greater than the error threshold, determine that the first target feature point and the second target feature point do not match.

[0093] Among them, when the reprojection error is represented by a pixel distance, the error threshold can be one of the pixel distances formed by 5 - 10 pixel points.

[0094] For example, when the error threshold is the pixel distance formed by 5 pixel points, then, when determining whether the feature point A1 and the feature point B1 corresponding to the three-dimensional space point C1 match, if both the first reprojection error W1 and the second reprojection error W2 are less than or equal to the pixel distance formed by 5 pixel points, determine that the feature point A1 and the feature point B1 are matching feature points. If any one of the first reprojection error W1 and the second reprojection error W2 is greater than the pixel distance formed by 5 pixel points, determine that the feature point A1 and the feature point B1 do not match.

[0095] In some embodiments, when the feature point A1 and the feature point B1 do not match, to facilitate screening out the feature points in the second image that match the feature point A1 in the first image, the embodiments of the present application screen out the matching feature points in the first image and the second image according to a preset matching principle. The preset matching principle can adopt a brute-force matching method, and sequentially traverse each feature point in the first image and each feature point in the second image in the order from left to right to select the feature points in the second image that match any feature point in the first image, and delete all the feature points in the first image and the second image that do not have a matching relationship.

[0096] Among them, the preset matching principle may specifically include: if the first target feature point and the second target feature point do not match, then select the next feature point in the second image and perform a matching verification with the first target feature point in the first image again. If no feature point matching the first target feature point can be traversed in the second image, then delete the first target feature point. And traverse again from the next feature point in the first image and the specified feature point in the second image, where the specified feature point in the second image refers to the feature point that has no matching relationship with the first target feature point. During the process of traversing the feature points of the second image, if a second target feature point matching the first target feature point is obtained, then stop traversing the second image; then traverse from the next feature point that matches successfully in the first image and the next feature point that matches successfully in the second image. After all the feature points in the first image and all the feature points in the second image have been traversed, stop the matching verification process, determine the feature points with a matching relationship in the first image and the second image as matching feature points, and delete all the feature points without a matching relationship in the first image and the second image.

[0097] For example, when determining whether the feature point A1 in the first image and the feature point B1 in the second image match, if it is determined that the feature point A1 and the feature point B1 do not match, then obtain the feature point B2 in the second image and perform a matching verification on the feature point A1 in the first image and the feature point B2 in the second image. If no feature point matching the feature point A1 in the first image can be found after traversing all the feature points in the second image, then delete the feature point A1 in the first image. Next, select the feature point A2 in the first image and perform a matching verification with the feature point B1 in the second image. If the feature point A2 in the first image and the feature point B1 in the second image match successfully, then determine the feature point A2 and the feature point B1 as matching feature points. Next, obtain the feature point A3 in the first image and perform a matching verification with the feature point B2 in the second image. And so on. After all the feature points in the first image and all the feature points in the second image have been traversed, stop the matching verification process. It should be noted that the process of matching verification is the implementation process of steps S3 to S4 and related solutions in the foregoing embodiments, which will not be elaborated here.

[0098] That is to say, when performing feature point matching between the first image and the second image, each time a three-dimensional space point is reconstructed based on a set of feature points, it is back-projected once. In this way, after obtaining the correct three-dimensional point by back-projection, stop the three-dimensional reconstruction of this two-dimensional point in the left view and other two-dimensional points in the right view, and start the three-dimensional reconstruction and back-projection of the next two-dimensional point in the left view and the next two-dimensional point in the right view, and so on, until all the matching feature points in the first image and the second image are screened out.

[0099] In the embodiment of the present application, first, the feature points selected from the two images are triangulated for 3D reconstruction to form 3D space points. Then, the 3D space points are back-projected onto each image to calculate the reprojection error generated by the 3D space points in each image. The reprojection error calculated through the two projection processes is used to perform matching verification on the feature points in the two images, and the matching feature points used to represent the same object can be screened out, and the mis-matched feature points can be eliminated. The 3D space points corresponding to the matching feature points can be determined as 3D points, and by reconstructing the 3D points, the accuracy of the matching feature points in the two images representing the same object can be improved, thereby ensuring the accuracy of the subsequent determination of the pose information of the calibrator.

[0100] S5. Based on a preset rule, the 3D points are formed into a calibrator, and the calibrator is matched with a standard calibrator to obtain the pose information of the calibrator.

[0101] Among them, the preset rule may include forming a calibrator from the 3D points according to the target number of circular markers required to form the calibrator.

[0102] After screening out multiple 3D points according to the methods shown in steps S3 and S4, all the 3D points are formed into at least one calibrator according to the target number of circular markers required to form the calibrator. Through calibrator matching, the rotation and translation relationship between the formed calibrator and the standard calibrator is calculated to determine the pose information of the calibrator. Among them, the calibrator is used to represent the real-time constructed calibrator model, and the standard calibrator refers to the model of the calibrator in the reference form.

[0103] Figure 7 It is a flowchart of the method for forming a calibrator from 3D points provided by an exemplary embodiment of the present application. As Figure 7 shown, in some embodiments, based on a preset rule, the process of forming a calibrator from 3D points includes:

[0104] S51. In 3D space, any two of the 3D points are connected to form a line segment.

[0105] S52. Calculate the line segment distance of each line segment, and calculate the angle between any two line segments.

[0106] S53. Obtain the standard line segment distance and standard angle of the standard calibrator. If the line segment distance and angle formed by the specified 3D point and the associated 3D point simultaneously meet the standard line segment distance and standard angle conditions, the specified 3D point is determined as the target 3D point, and the associated 3D point refers to the 3D point connected to the specified 3D point to form a line segment.

[0107] S54. Based on the target 3D points and the corresponding associated 3D points, a calibrator is formed according to the standard calibrator constraint rule.

[0108] Ideally, if the first image and the second image captured by the binocular camera only include images of circular markers, all the three-dimensional points constructed in the embodiments of the present application are three-dimensional points representing circular markers, and thus a calibrator can be formed. However, if the first image and the second image include images of circular markers and impurity targets, all the three-dimensional points constructed in the embodiments of the present application include three-dimensional points representing circular markers and three-dimensional points representing impurity targets, and thus several calibrators can be formed.

[0109] To accurately form a calibrator based on the three-dimensional points, it is necessary to perform denoising processing on all the three-dimensional points, delete the three-dimensional points representing impurity targets, and form a calibrator based on the three-dimensional points representing circular markers. Therefore, to facilitate the construction of a calibrator, it is necessary to screen out the target three-dimensional points that can form a calibrator meeting the requirements of the standard calibrator from several three-dimensional points, and then construct a calibrator based on the target three-dimensional points.

[0110] The process of screening three-dimensional points can be equivalent to the process of calibrator matching. Calibrator matching is to match the calibrator formed by the three-dimensional points constructed in real time with the standard calibrator, that is, to match each three-dimensional point on the calibrator with the points of the standard calibrator in the reference form, and, to match the angles between the line segments formed by each point of the calibrator with the angles between the line segments formed by each point of the standard calibrator in the reference form.

[0111] When screening the target three-dimensional points, the matching of points and the matching of angles between line segments can be performed simultaneously; or, first perform the matching of points, and screen out the points that meet the point matching; then form several line segments from any two of the screened points to perform the matching of angles between each line segment. The two matching principles can be determined based on the actual application and are not limited here.

[0112] Figure 8 It is a schematic diagram of forming a calibrator based on three-dimensional points provided for an exemplary embodiment of the present application. As Figure 8 shown, taking the simultaneous matching of points and the matching of angles between line segments as an example, among several reconstructed three-dimensional points, connect any two three-dimensional points into a line segment. For example, if there are 4 three-dimensional points D1 - D4, when determining whether the specified three-dimensional point D1 is a target three-dimensional point, the specified three-dimensional point D1 and other associated three-dimensional points (D2, D3, D4) can form three line segments, namely the line segment formed by point D1 and point D2 is D1D2, the line segment formed by point D1 and point D3 is D1D3, and the line segment formed by point D1 and point D4 is D1D4. Calculate the line segment distance of each line segment L1 = D1D2, L2 = D1D3, L3 = D1D4. Calculate the angle α1 between the line segments D1D2 and D1D3, calculate the angle α2 between the line segments D1D3 and D1D4, and calculate the angle α3 between the line segments D1D2 and D1D4.

[0113] Match the line segment distances (L1, L2, L3) formed by the specified three-dimensional point D1 with the standard line segment distance L0 corresponding to point i in the standard calibrator, and match the angles (α1, α2, α3) formed by the specified three-dimensional point D1 with the standard angle α0 corresponding to point i in the standard calibrator. i When the standard calibrator consists of 4 circular markers, there are four corresponding points. Each point will form three line segments, obtaining three line segment distances, and the three line segments can form three angles. Set the standard line segment distance condition as the distance error threshold being less than or equal to 2%, and set the standard angle condition as the angle error threshold being less than or equal to 2%. i If the distance errors between the three line segment distances (L1, L2, L3) corresponding to the specified three-dimensional point D1 and the associated three-dimensional points (D2, D3, D4) and the standard line segment distances (L0

[0114] , L0 A1 , L0 A2 , L0 A3 ) corresponding to point A in the standard calibrator are less than or equal to 2%, then it is determined that the three line segment distances corresponding to the specified three-dimensional point D1 satisfy the standard line segment distance condition; otherwise, they do not satisfy the standard line segment distance condition. If the angle errors between the three angles (α1, α2, α3) corresponding to the specified three-dimensional point D1 and the associated three-dimensional points (D2, D3, D4) and the standard angles (α0 A1 , α0 A2 , α0 A3 ) corresponding to point A in the standard calibrator are less than or equal to 2%, then it is determined that the three angles corresponding to the specified three-dimensional point D1 satisfy the standard angle condition; otherwise, they do not satisfy the standard angle condition.

[0115] Therefore, if the three line segment distances and the three angles corresponding to the specified three-dimensional point D1 simultaneously satisfy the standard line segment distance and the standard angle conditions, then the specified three-dimensional point D1 is determined as the target three-dimensional point. Since the line segment distances and angles formed by the standard calibrator are unique, after selecting the target three-dimensional points belonging to the calibrator, the calibrator can be determined based on the target three-dimensional points and the corresponding associated three-dimensional points, that is, the three-dimensional points D1 - D4 can form a calibrator according to the standard calibrator constraint rules. If any one of the three line segments and the three angles corresponding to the specified three-dimensional point D1 and the associated three-dimensional points does not satisfy the standard line segment distance and the standard angle conditions, then the three-dimensional point D1 is determined as an impurity point and the three-dimensional point D1 is deleted.

[0116] In some embodiments, after the calibrator is constructed based on the target three-dimensional points, the calibrator can be matched with the standard calibrator to obtain the pose information of the calibrator. This process includes: using the Iterative Closest Point (ICP) algorithm to perform registration calculations on the calibrator and the corresponding standard calibrator to obtain the pose information of the calibrator, where the pose information includes the translation value t and the rotation value R of the calibrator.

[0117] Among them, the Iterative Closest Point (ICP) algorithm is a registration method based on free-form surfaces. The ICP algorithm is based on the data registration method and uses the nearest point search method to solve an algorithm based on free-form surfaces.

[0118] In some embodiments, when using the ICP algorithm to match the calibrator, for the points p i (i = 1...n) in the calibrator and the points p i ’(i = 1...n) in the standard calibrator are matched and calculated, and each point is a column vector. Calculate the centers of the calibrator and the standard calibrator to obtain the vectors p c , p c ’; and remove the centers to obtain the vectors q i , q i ’.

[0119] According to the formula: Determine the rotation value R of the calibrator. In the formula, R * is an orthogonal matrix. And, according to the formula: p c = Rp c '+ t, determine the translation value t of the calibrator.

[0120] In some embodiments, usually one standard calibrator corresponds to one calibrator. Then, if there is a situation where one standard calibrator corresponds to multiple calibrators each composed of 4 three-dimensional points, the embodiments of the present application determine the pose information of the calibrator based on the calibrator with the minimum matching error.

[0121] The calibrator matching method provided by the embodiments of the present application further includes: when the standard calibrator corresponds to multiple calibrators, calculate the calibrator matching error corresponding to each calibrator based on the translation value and rotation value of each calibrator. Screen out the target calibrator corresponding to the minimum calibrator matching error, and determine the pose information of the calibrator based on the translation value and rotation value of the target calibrator.

[0122] According to the formula: Calculate the calibrator matching error of each calibrator. And select the target calibrator that generates the minimum calibrator matching error. Then, the pose information of the calibrator includes the translation value and rotation value of the target calibrator.

[0123] Figure 9Schematic diagram of an image captured by a binocular camera provided for an exemplary embodiment of the present application. As Figure 9 shown, in some embodiments, the relative positional relationship of each circular marker imaged in the left and right view images captured by the binocular camera ( Figure 9 the white circular objects shown by reference numerals 1-4 in the figure) is related to the relative positional relationship of each actual circular marker. Then, when performing 3D point reconstruction, if no constraints are added, the corresponding point of a feature point in the left view image in the right view image needs to be searched through the right view image, which results in low search efficiency.

[0124] To improve the traversal search efficiency, in the embodiment of the present application, after performing step S2 (extracting feature points) and before step S3 (reconstructing 3D space points), epipolar rectification processing is performed on the left and right view images. The purpose of epipolar rectification is to convert the positions of the same 3D point in the left and right view images to the same row, so that the traversal search can be reduced from two dimensions to one dimension, greatly improving the search efficiency. Exemplarily, the embodiment of the present application can use the stereoRectify function in OpenCV to perform epipolar rectification operations.

[0125] In some embodiments, before step S3, that is, before triangulating and reconstructing the first target feature point in the first image and the second target feature point in the second image to obtain the corresponding 3D space point, the calibration marker matching method provided by the embodiment of the present application further includes: performing epipolar rectification processing on the first image and the second image, and the epipolar rectification processing is used to convert the positions of the feature points representing the same circular marker in the first image and the second image to the same row.

[0126] Combined with step S2 in the foregoing embodiments, after the feature point extraction process of the first image and the second image in step S2 is performed, epipolar processing is performed on the first image and the second image. After epipolar rectification processing, the feature points representing the same circular marker in the first image and the second image are located on the same horizontal epipolar line. For the effect diagrams of the first image and the second image after epipolar rectification processing, see Figure 4 and Figure 5 the content shown. After the epipolar rectification processing of the first image and the second image is completed, the subsequent process of step S3 of reconstructing 3D space points is performed.

[0127] In some embodiments, when reconstructing three-dimensional space points and filtering out three-dimensional points based on feature point matching, the first target feature points in the first image and the second target feature points in the second image required for establishing the three-dimensional space points are both feature points located on the same row. For example, corresponding three-dimensional space points are established based on the first target feature points in the first image and the second target feature points in the second image on the same row. Then, subsequent back-projection and reprojection error calculations are performed to determine the three-dimensional points. The specific process can refer to the content of steps S3 to S4 and related steps in the foregoing embodiments, which will not be elaborated here.

[0128] In some embodiments, when reconstructing three-dimensional space points by selecting feature points in the left and right view images based on a preset matching principle, when the feature points in the second image are traversed and matched for the first target feature points in the first image, the traversal range is all the feature points in the second image on the same row as the first target feature points, so as to improve the search efficiency. The specific process can refer to the relevant content of steps S41 - S42 and steps S421 - S424 in the foregoing embodiments, which will not be elaborated here.

[0129] In some embodiments, after epipolar rectification processing, the number of rows in the left and right view images is theoretically the same. However, due to the error of camera calibration, the rows where the feature points representing the same object are located in the first image do not correspond to the rows in the second image, resulting in inconsistent numbers of feature points on the same row in the first image and the second image, and thus low feature point search efficiency. Therefore, to ensure the search efficiency of feature point matching, different strategies are executed according to different situations after obtaining the feature points on the same row.

[0130] When the numbers of feature points in the first image and the second image on the same row are the same, it indicates that the feature points in the left and right view images are corresponding matching points from left to right. Then, they can be matched in order to reconstruct the three-dimensional points of the subsequent circular markers.

[0131] When the numbers of feature points in the first image and the second image on the same row are inconsistent, it is necessary to remove the redundant feature points in the image with redundant feature points according to the positional relationship of the feature points on the same row, so that the numbers of feature points in the first image and the second image on the same row are kept the same.

[0132] In some embodiments, before triangulating and reconstructing the first target feature points in the first image and the second target feature points in the second image to obtain the corresponding three-dimensional space points, the calibrator matching method provided by the embodiments of the present application further includes: after epipolar rectification processing, obtaining the number of feature points on each row in the first image and the number of feature points on each row in the second image. On the same row, if the number of corresponding feature points in the first image is different from the number of corresponding feature points in the second image, then based on the positional relationship between the feature points in the first image and the feature points in the second image on the same row, the redundant feature points generated after feature point comparison in the first image and the second image are deleted according to a preset comparison rule, so that the number of corresponding feature points in the first image is the same as the number of corresponding feature points in the second image on the same row, and the step of triangulating and reconstructing the first target feature points in the first image and the second target feature points in the second image to obtain the corresponding three-dimensional space points is performed.

[0133] Wherein, the preset comparison rule refers to the strategy of deleting feature points starting from the direction indicated by the position of the view. For example, for the left view image, the feature points are deleted starting from the leftmost side; for the right view image, the feature points are deleted starting from the rightmost side.

[0134] Figure 10 It is a schematic diagram of the inconsistent number of feature points in the left and right view images on the same row provided by the exemplary embodiment of the present application. As Figure 10 shown, on the same row (epipolar line), if the number of feature points in the left view image (such as 4) is more than the number of feature points in the right view image (such as 3), then the leftmost feature point is removed from the left view image until the number of corresponding feature points in the first image is the same as the number of corresponding feature points in the second image on the same row. The number of removed feature points is the difference between the number of feature points in the left view image and the number of feature points in the right view image. For example, the leftmost first feature point in the left view image is deleted.

[0135] On the same row, if the number of feature points in the right view image is more than the number of feature points in the left view image, then the rightmost feature point is removed from the right view image until the number of corresponding feature points in the first image is the same as the number of corresponding feature points in the second image on the same row.

[0136] After completing the epipolar rectification processing and removing redundant feature points, the subsequent matching method can be executed. Through the epipolar rectification processing and removing redundant feature points, not only can the matching speed be improved, but also the three-dimensional points with reconstruction errors can be effectively removed, ensuring the accuracy and efficiency of the subsequent calibrator matching algorithm.

[0137] A calibration device matching method provided by an embodiment of the present application obtains a first image and a second image collected by a binocular camera, and extracts feature points for characterizing circular markers in the two images; triangulates and reconstructs the first target feature points in the first image and the second target feature points in the second image to obtain corresponding three-dimensional space points; filters out the matching feature points of the first image and the second image based on the three-dimensional space points, and determines the three-dimensional space points corresponding to the generated matching feature points as three-dimensional points; forms a calibration device based on the three-dimensional points according to a preset rule, and matches the calibration device with a standard calibration device to obtain the pose information of the calibration device. It can be seen that this method eliminates mis-matched feature points through triangulation reconstruction, avoids the problem of difficult feature point matching in the left and right view images, accurately constructs three-dimensional points, realizes stable low-texture calibration device reconstruction, and then accurately determines the pose information of the calibration device.

[0138] Figure 11 It is a structural block diagram of a calibration device matching device provided by an exemplary embodiment of the present application. As Figure 11 shown, an embodiment of the present application provides a calibration device matching device, including:

[0139] A data acquisition module 10, configured to acquire a first image and a second image collected by a binocular camera, and both the first image and the second image include images of several circular markers;

[0140] A feature point extraction module 20, configured to extract feature points in the first image and feature points in the second image, and the feature points are used to characterize circular markers;

[0141] A three-dimensional space point reconstruction module 30, configured to triangulate and reconstruct the first target feature points in the first image and the second target feature points in the second image to obtain corresponding three-dimensional space points;

[0142] A three-dimensional point determination module 40, configured to filter out the matching feature points of the first image and the second image based on the three-dimensional space points, and determine the three-dimensional space points corresponding to the generated matching feature points as three-dimensional points, where the matching feature points refer to the feature points in the first image and the second image that characterize the same circular marker

[0143] A pose information determination module 50, configured to form a calibration device based on the three-dimensional points according to a preset rule, and match the calibration device with a standard calibration device to obtain the pose information of the calibration device.

[0144] A calibration device matching method and apparatus provided by an embodiment of the present application obtain a first image and a second image collected by a binocular camera, and extract feature points for characterizing circular markers in the two images; triangulate and reconstruct a first target feature point in the first image and a second target feature point in the second image to obtain corresponding three-dimensional space points; screen out matching feature points of the first image and the second image based on the three-dimensional space points, and determine the three-dimensional space points corresponding to the generated matching feature points as three-dimensional points; form a calibration device from the three-dimensional points based on a preset rule, and match the calibration device with a standard calibration device to obtain pose information of the calibration device. It can be seen that this method and apparatus eliminate mis-matched feature points through triangulation reconstruction, avoid the problem of difficult feature point matching in left and right view images, accurately construct three-dimensional points, achieve stable low-texture calibration device reconstruction, and further accurately determine the pose information of the calibration device.

[0145] For the same and similar parts among the various embodiments in this specification, reference can be made to each other. In particular, for the embodiment of the calibration device matching apparatus, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can refer to the description in the method embodiment.

[0146] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit them; although the present application has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements on some or all of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the various embodiments of the present application.

[0147] For the sake of convenience of explanation, the above description has been made in combination with specific implementation manners. However, the above exemplary discussion is not intended to be exhaustive or to limit the implementation manners to the specific forms disclosed above. According to the above teachings, various modifications and variations can be obtained. The selection and description of the above implementation manners are for better explaining the present disclosure, so that those skilled in the art can better use the implementation manners.

Claims

1. A calibrator matching method, characterized in that, Including: Obtain the acquired first image and second image, where the first image and the second image both include images of several circular markers; Extract the feature points in the first image and the feature points in the second image, where the feature points are used to represent the circular markers; Triangulate and reconstruct the first target feature points in the first image and the second target feature points in the second image to obtain corresponding three-dimensional space points; Based on the three-dimensional space points, screen out the matching feature points of the first image and the second image, and determine the three-dimensional space points corresponding to the generated matching feature points as three-dimensional points. The matching feature points refer to the feature points in the first image and the second image that represent the same circular marker; Based on a preset rule, form a calibrator with the three-dimensional points, and match the calibrator with a standard calibrator to obtain the pose information of the calibrator. Including: In three-dimensional space, connect any two of the three-dimensional points to form line segments; Calculate the line segment distances of each line segment, and calculate the angles between any two line segments; Obtain the standard line segment distance and standard angle of the standard calibrator. If the line segment distance and angle formed by the specified three-dimensional point and the associated three-dimensional point simultaneously meet the standard line segment distance and standard angle conditions, then determine the specified three-dimensional point as the target three-dimensional point. The associated three-dimensional point refers to the three-dimensional point connected to the specified three-dimensional point to form a line segment; Based on the target three-dimensional point and the corresponding associated three-dimensional point, form a calibrator according to the standard calibrator constraint rule; Use the iterative closest point algorithm to perform registration calculation on the calibrator and the corresponding standard calibrator to obtain the pose information of the calibrator. The pose information of the calibrator includes the translation value and rotation value of the calibrator.

2. The method according to claim 1, characterized in that, The screening out the matching feature points of the first image and the second image based on the three-dimensional space points includes: Back-project the three-dimensional space points into the first image to form first projection points, and back-project the three-dimensional space points into the second image to form second projection points; If the first projection point matches the first target feature point, and the second projection point matches the second target feature point, then determine the first target feature point and the second target feature point as matching feature points.

3. The method according to claim 2, characterized in that, The if the first projection point matches the first target feature point, and the second projection point matches the second target feature point, then determine the first target feature point and the second target feature point as matching feature points includes: Calculate the first reprojection error between the first projection point and the first target feature point, and calculate the second reprojection error between the second projection point and the second target feature point; Compare the first reprojection error with an error threshold, and compare the second reprojection error with the error threshold; If both the first reprojection error and the second reprojection error are less than or equal to the error threshold, then determine the first target feature point and the second target feature point corresponding to the three-dimensional space points as matching feature points; If any one of the first reprojection error and the second reprojection error is greater than the error threshold, then determine that the first target feature point and the second target feature point do not match.

4. The method according to claim 3, wherein The method further includes: When the first target feature point and the second target feature point do not match, according to the preset matching principle, matching feature points are screened out in the first image and the second image. The preset matching principle includes traversing each feature point in the first image and each feature point in the second image in sequence from left to right, so as to select, in the second image, a feature point that matches any feature point in the first image, and deleting all feature points in the first image and the second image that do not have a matching relationship.

5. The method according to claim 4, wherein The screening out of matching feature points in the first image and the second image according to the preset matching principle includes: When the first target feature point and the second target feature point do not match, select the next feature point in the second image and perform a matching verification with the first target feature point; If no feature point matching the first target feature point can be traversed in the second image, delete the first target feature point and traverse again from the next feature point in the first image and the specified feature point in the second image, where the specified feature point in the second image refers to the feature point in the second image that does not have a matching relationship with the first target feature point; During the process of traversing the feature points of the second image, if a second target feature point matching the first target feature point is obtained, stop traversing the second image, and traverse from the next feature point that matches successfully in the first image and the next feature point that matches successfully in the second image; After all feature points in the first image and all feature points in the second image have been traversed, stop the matching verification process, determine the feature points with a matching relationship in the first image and the second image as matching feature points, and delete all feature points in the first image and the second image that do not have a matching relationship.

6. The method according to claim 1, wherein Before triangulating and reconstructing the first target feature point in the first image and the second target feature point in the second image to obtain the corresponding three-dimensional space point, the method further includes: Performing epipolar rectification processing on the first image and the second image, and the epipolar rectification processing is used to convert the positions of the feature points representing the same circular marker in the first image and the second image to the same row.

7. The method according to claim 6, wherein Before triangulating and reconstructing the first target feature point in the first image and the second target feature point in the second image to obtain the corresponding three-dimensional space point, the method further includes: After the epipolar rectification processing, obtain the number of feature points on each row in the first image and the number of feature points on each row in the second image; On the same line, if the number of corresponding feature points in the first image is different from that in the second image, based on the positional relationship between the feature points in the first image and the feature points in the second image on the same line, the redundant feature points generated after feature point comparison in the first image and the second image are deleted according to a preset comparison rule, so that the number of corresponding feature points in the first image is the same as that in the second image on the same line, and the step of triangulating and reconstructing the first target feature point in the first image and the second target feature point in the second image to obtain the corresponding three-dimensional space points is performed.

8. The method according to claim 1, wherein The method further includes: When the standard calibrator corresponds to multiple calibrators, based on the translation value and rotation value of each calibrator, calculate the calibrator matching error corresponding to each calibrator; Screen out the target calibrator corresponding to the smallest calibrator matching error, and based on the translation value and rotation value of the target calibrator, determine the pose information of the calibrator.

9. A calibrator matching device, characterized in that, It includes: A data acquisition module, configured to acquire a first image and a second image collected by a binocular camera, where both the first image and the second image include images of several circular markers; A feature point extraction module, configured to extract the feature points in the first image and the feature points in the second image, where the feature points are used to represent circular markers; A three-dimensional space point reconstruction module, configured to triangulate and reconstruct the first target feature point in the first image and the second target feature point in the second image to obtain the corresponding three-dimensional space points; A three-dimensional point determination module, configured to screen out the matching feature points of the first image and the second image based on the three-dimensional space points, and determine the three-dimensional space points corresponding to the generated matching feature points as three-dimensional points, where the matching feature points refer to the feature points representing the same circular marker in the first image and the second image; A pose information determination module, configured to form a calibrator with the three-dimensional points based on a preset rule, match the calibrator with a standard calibrator, and obtain the pose information of the calibrator; it includes: In three-dimensional space, connect any two of the three-dimensional points into a line segment; Calculate the line segment distance of each line segment, and calculate the angle between any two line segments; Obtain the standard line segment distance and standard angle of the standard calibrator. If the line segment distance and angle formed by the specified three-dimensional point and the associated three-dimensional point simultaneously meet the standard line segment distance and standard angle conditions, then determine the specified three-dimensional point as the target three-dimensional point, where the associated three-dimensional point refers to the three-dimensional point connected to the specified three-dimensional point to form a line segment; Based on the target three-dimensional point and the corresponding associated three-dimensional point, form a calibrator according to the standard calibrator constraint rule; Use the iterative closest point algorithm to perform registration calculation on the calibrator and the corresponding standard calibrator to obtain the pose information of the calibrator, where the pose information of the calibrator includes the translation value and rotation value of the calibrator.

Images