Motion recovery structure center point detection method and device based on color threshold segmentation and storage medium

Abstract

The invention discloses a method and a device for detecting a central point of a motion recovery structure (SfM) based on color threshold segmentation, and a storage medium, and belongs to the technical field of image processing. The method mainly aims at the problem that the extraction and fitting accuracy of target circular ring contour feature points is limited under the influence of dynamic flight and complex environmental factors of an unmanned aerial vehicle, and comprises the following steps: firstly, fitting circular ring contours in images, and removing images which do not meet technical standards; then, key information such as the three-dimensional coordinates, the direction vector and the yaw angle of the circle center is extracted through the color threshold segmentation auxiliary SfM technology; and finally, executing the same processing flow on a subsequent image, and correcting the initial three-dimensional center coordinate of the image by using the newly obtained three-dimensional center coordinate of the circle center so as to enhance the robustness of the three-dimensional center coordinate of the image. The technology is especially suitable for the application scene that the unmanned aerial vehicle passes through the circular ring task. The method is simple in calculation process, high in cost effectiveness and wide in applicability.

Description

Technical Field

[0001] The present invention belongs to the technical field of image processing, and in particular relates to a method, a device and a storage medium for detecting a center point of a motion recovery structure based on color threshold segmentation. Background Art

[0002] Traditional Structure from Motion (SfM) methods usually involve a lot of calculations, especially when processing large-scale image data, which leads to high computational complexity and high requirements on computing resources; they usually generate sparse 3D point clouds, while in some applications, more dense and accurate 3D reconstruction results are required. This sparsity may limit the application of SfM technology in some practical scenarios.

[0003] Furthermore, traditional SfM methods are sensitive to changes in illumination and texture in images. Therefore, in the presence of illumination changes or texture loss, the method may face difficulties, resulting in reduced accuracy; and usually requires good initial camera parameter estimation and feature matching, and these dependencies may be affected by noise and mismatches, thus affecting the entire reconstruction process. In addition, traditional SfM methods are usually based on the assumption of rigid body motion, so they may perform poorly when dealing with motion blur and non-rigid scenes.

[0004] In summary, due to the influence of the movement mode and environment of the UAV, the positioning effect obtained by the traditional SfM technology will be poor. Therefore, how to improve the accuracy and robustness of the system's pose estimation is an urgent problem that needs to be solved. Summary of the invention

[0005] In view of the above problems, the present invention provides a method for detecting center points of motion-recovered structures based on color threshold segmentation, so as to solve the above problems existing in the prior art.

[0006] According to one aspect of an embodiment of the present invention, a method for detecting center points of structure from motion based on color threshold segmentation is provided. The method uses an image taken by a camera for SfM analysis. The method specifically includes the following steps:

[0007] Step 1: Camera calibration: The camera calibration matrix is ​​used to realize the mutual conversion between the world coordinate system and the pixel coordinate system.

[0008] Step 2: Get RGB image: Get the circular image information from the camera, such as the image rotation matrix, color image, pose information, etc.

[0009] Step 3, obtain the ellipse contour: obtain the corresponding circular image contour based on the red threshold, and extract the array data, rotation matrix information, position information, and target data of the RGB circular image multi-channel. If the number of contour points of the camera's RGB circular image is less than the preset threshold, such as less than 10, skip the current image and do not perform subsequent ellipse fitting;

[0010] Step 4: Projection transformation: Describe the outer contour of the ring in 3D space; the specific process is as follows:

[0011] The three-dimensional contour points of the ring image obtained in step 3 are projected onto the camera's image plane through the camera's projection matrix to obtain the corresponding two-dimensional image coordinates. Next, three copies of the image processed in step 3 are created, and the image contours are depicted in green, yellow, and blue colors on these copies respectively;

[0012] Step 5: Image selection: Screen the images processed in step 4 to remove incomplete images and images with multiple similar targets.

[0013] Step 6: Pre-providing of ring information: providing the approximate three-dimensional coordinates of the center point, direction vector and yaw angle of the target ring image. The specific process is as follows:

[0014] Assume that the approximate 3D coordinates, direction vector and yaw angle of the center point of the given target circular ring image are the 3D coordinates, direction vector and yaw angle of the first point, so as to be fused with the 3D coordinates of the center point obtained from the same circular ring image, in order to obtain the 3D coordinates of the center point with smaller error;

[0015] Step 7: Calculate the parameters of the fitting graph:

[0016] Based on the 3D image coordinates of the torus, its position and orientation in space are calculated. First, it fits the plane containing the torus and determines the rotation vector and yaw angle required to rotate the plane to be parallel to the x-axis. Then, using the Rodriguez rotation formula, these rotation parameters are applied to the 3D point set to transfer it to a new coordinate system. In this new coordinate system, a circle is fit to the 3D point set to determine the center and radius of the circle. Finally, the algorithm calculates the world coordinates of the torus center, the rotation vector, the yaw angle, and the radius of the torus;

[0017] Step 8, Update SfM: Use the Rodriguez formula to calculate the direction vector and yaw angle information of the new center of the same target ring to obtain more accurate ring information, update the 3D coordinates of the image, map the world coordinates to the camera coordinates, and finally return the updated world coordinates;

[0018] Step 9, iterate the three-dimensional coordinates of the center of the circle: iterate the three-dimensional image center point of the target circle through steps 1 to 8, analyze multiple three-dimensional coordinates of the center of the same target circle, direction vectors, and yaw angles, and obtain more accurate three-dimensional coordinates of the center of the circle.

[0019] According to one aspect of an embodiment of the present invention, a motion recovery structure center point detection system based on color threshold segmentation is provided, which is used to implement any of the above methods.

[0020] According to one aspect of an embodiment of the present invention, a method for UAV path planning is provided, comprising: calculating the three-dimensional world coordinates of the target ring according to any of the above-mentioned motion recovery structure center point detection methods based on color threshold segmentation, so that the UAV uses this as the planning end point when planning the path.

[0021] According to one aspect of an embodiment of the present invention, there is provided an electronic device, comprising:

[0022] one or more processors;

[0023] A memory for storing one or more programs;

[0024] When the one or more programs are executed by the one or more processors, the one or more processors implement any of the methods described above.

[0025] According to one aspect of an embodiment of the present invention, a computer-readable storage medium is provided, storing computer-executable instructions, wherein the instructions are used to implement any of the above-mentioned methods when executed.

[0026] The beneficial effects of the present invention are:

[0027] The present invention utilizes the image center point detection method, system and device based on motion structure reconstruction, uses the function of the open source computer vision library (OpenCV), and clearly displays the outline of the ring in the image taken by the drone camera in yellow, blue and green colors, and presents the point cloud image of the ring outline through the function. At this stage, the ring outline image that does not meet the conditions can be deleted in advance, so as to provide more accurate and more robust three-dimensional coordinates of the center of the ring for subsequent drone path planning. At the same time, the method of motion recovery structure is adopted to extract key information such as the three-dimensional coordinates of the center of the outer ring, the direction vector and the yaw angle by continuously shooting two-dimensional camera images. Through iterative optimization, the images captured by the camera are continuously parsed to obtain the information such as the three-dimensional coordinates of the center of the same target ring, the direction vector and the yaw angle, so as to more accurately correct the three-dimensional coordinates of the center of the ring. This provides a more accurate basis for subsequent drone path planning, ensuring that the drone can maintain stable control when crossing the ring. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 is a system block diagram of the method of the present invention;

[0029] Figure 2 It is a schematic diagram of the image screening method;

[0030] Figure 3 It is the system SfM structure diagram. DETAILED DESCRIPTION

[0031] The technical solution of the present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments.

[0032] According to an embodiment of the present invention, a method for detecting center points of motion-recovered structures based on color threshold segmentation is provided. Figure 1 As shown, the method specifically comprises the following steps:

[0033] Step 1: Camera calibration: The camera calibration matrix is ​​used to realize the mutual conversion between the world coordinate system and the pixel coordinate system.

[0034] Step 2: Get RGB image: Get the circular image information from the camera, such as the image rotation matrix, color image, pose information, etc.

[0035] Step 3, obtain the ellipse contour: obtain the corresponding circular image contour based on the red threshold, and extract the array data, rotation matrix information, position information, and target data of the RGB circular image multi-channel. If the number of contour points of the camera's RGB circular image is less than 10, skip the current image and do not perform subsequent ellipse fitting;

[0036] Step 4: Projection transformation: Describe the outer contour of the ring in 3D space; the specific process is as follows:

[0037] Project the three-dimensional points of the ring image obtained in step 3 onto the camera's image plane through the camera's projection matrix to obtain the corresponding two-dimensional image coordinates. Next, create three copies of the image processed in step 3, and use green, yellow, and blue colors to depict the image outline on these copies;

[0038] Step 5: Image selection: Figure 2 As shown, the images processed in step 4 are screened to remove incomplete images and the situation where there are multiple similar targets in one image;

[0039] Step 6: Pre-providing of ring information: providing the approximate three-dimensional coordinates of the center point, direction vector and yaw angle of the target ring image. The specific process is as follows:

[0040] Assume that the approximate 3D coordinates, direction vector and yaw angle of the center point of the given target circular ring image are the 3D coordinates, direction vector and yaw angle of the first point, so as to be fused with the 3D coordinates of the center point obtained from the same circular ring image, in order to obtain the 3D coordinates of the center point with smaller error;

[0041] Step 7: Calculate the parameters of the fitting graph:

[0042] Based on the 3D image coordinates of the torus, its position and orientation in space are calculated. First, it fits the plane containing the torus and determines the rotation vector and yaw angle required to rotate the plane to be parallel to the x-axis. Then, using the Rodriguez rotation formula, these rotation parameters are applied to the 3D point set to transfer it to a new coordinate system. In this new coordinate system, a circle is fit to the point set to determine the center and radius of the circle. Finally, the algorithm calculates the world coordinates of the torus center, the rotation vector, the yaw angle, and the radius of the torus;

[0043] Step 8, Update SfM: Use the Rodriguez formula to calculate the direction vector and yaw angle information of the new center of the same target ring to obtain more accurate ring information, update the 3D coordinates of the image, map the world coordinates to the camera coordinates, and finally return the updated world coordinates;

[0044] Step 9, iterate the three-dimensional coordinates of the center of the circle: iterate the three-dimensional image center point of the target circle through steps 1 to 8, analyze multiple three-dimensional coordinates of the center of the same target circle, direction vectors, and yaw angles, and obtain more accurate three-dimensional coordinates of the center of the circle.

[0045] The present invention adopts a method of restoring structure from motion. By continuously shooting two-dimensional camera images, key information such as the three-dimensional coordinates of the center of the outer ring, the direction vector and the yaw angle are extracted. Through iterative optimization, the images captured by the camera are continuously analyzed to obtain the precise three-dimensional coordinates, direction vector and yaw angle of the same target ring, so as to more accurately correct the three-dimensional coordinates of the center of the ring. This provides a more accurate basis for subsequent UAV path planning, ensuring that the UAV can maintain stable control when crossing the ring. At the same time, the present invention uses the function of the open source computer vision library to clearly display the outline of the ring in the image taken by the drone camera in yellow, blue and green colors, and generates a point cloud image of the ring outline through the function. At this stage, we can pre-exclude the ring outline images that do not meet the conditions, thereby providing more accurate and robust three-dimensional coordinates of the center of the ring for subsequent UAV path planning. The method is simple to operate and highly operable, providing important parameters for the trajectory planning of subsequent UAVs.

[0046] Specifically, step 1 first sets a rotation matrix R from world coordinates to camera coordinates w2c and the camera intrinsic parameter K CAM , the two are multiplied by dot product to obtain the camera intrinsic parameter matrix K ck .

[0047] Step 3: Extract the red channel pixel value of the ellipse in the image and perform threshold processing. Set the pixel value of the pixel points that meet the range between 220 and 240 (i.e., the high-brightness red area) to 128, and set the other pixels that do not meet this range to 0 to generate a binary image. Find the contour on the binary image and return the contour list α cnts and the hierarchy α hierarchy , hierarchy α hierarchy Contains index information of upper and lower contours, ellipse contour α cnt Indicates traversal of the contour list α cnts For each contour in , if the number of points of the contour is greater than 10, the points fitting this contour become an ellipse, which contains the ellipse information, such as the center coordinates of the ellipse, the lengths of the major and minor axes, and the rotation angle of the ellipse. Add the ellipse information to β ellps List, add the profile information to β cnt_ret The binary image t after threshold processing is obtained. thresh , contour list β cnt_ret and the ellipse list β ellps .

[0048] Step 4: Convert the three-dimensional point W in the world coordinate system t Through the camera projection matrix K Rt Project it onto the image plane of the camera to obtain the corresponding two-dimensional image coordinates P t2d , the specific process is:

[0049] Step 4-1: The rotation matrix R and translation vector t of each frame of RGB ring image taken by the drone camera CAM Merge into the projection matrix, and then the camera intrinsic parameter matrix K ck Multiply it with the combined projection matrix to get the projection matrix K of the ideal camera Rt :

[0050] t CAM =R -1 [-(P+T CAM )] (41)

[0051] T proj = {R -1 ,t CAM} (42)

[0052] K Rt =K ck ·T proj (43)

[0053] where t CAM represents the camera's translation vector, R represents the camera's rotation matrix, P represents the camera's current position, and T CAM Represents the three-dimensional coordinates of the camera. The superscript -1 represents the inverse of the matrix. proj The matrix representing the camera rotation and translation, K Rt represents the camera projection matrix, K ck Represents the camera calibration matrix;

[0054] Step 4-2: The three-dimensional coordinates of the target object are passed through the camera's projection matrix K Rt Projection is performed, and finally the x and y coordinates of the projected three-dimensional coordinates of the target object are divided by the z coordinate to obtain the two-dimensional image coordinates. The specific two-dimensional image coordinate conversion is obtained by the following formula:

[0055] P 3d =K Rt ·[P wt ] T (44)

[0056]

[0057] Where P wt represents the three-dimensional coordinates of the target object, P 3d represents the three-dimensional coordinates of the ring to be traversed after the transformation, and the superscript T represents the transpose of the matrix. 3d ] 1,2 Indicates P 3d The x and y coordinates, [P 3d ]3 means P 3d The z coordinate, Pt2d Represents the two-dimensional image coordinates obtained by projecting the target object onto the image plane of the camera.

[0058] Specifically, step 7 is:

[0059] According to the three-dimensional coordinates of the approximate center point of the target ring image, the direction vector and the yaw angle are converted into the rotation matrix R through the Rodriguez formula r , calculate the coefficients abcd of the plane equation in camera coordinates, and transform the image coordinates of the image into world coordinates W t ;

[0060] Step 7-1: Obtain the direction vector v of the first target ring image according to step 6 ec , converted to the rotation matrix R through the Rodriguez formula r , the specific conversion method is obtained by the following formula:

[0061]

[0062] R r = cosθ yaw I+(1-cosθ yaw )[v ec ][v ec ] T +sinθ yaw [v ec ]^ (47)

[0063] where v ec Represents the direction vector of the first target ring image, [v ec ]1 represents the first row of direction vectors, [v ec ]2 represents the second row of direction vectors, [v ec ]3 represents the third row of the direction vector, I represents the 3x3 identity matrix, [v ec ]^ represents the antisymmetric matrix of the direction vector, R r represents the rotation matrix after the Rodriguez formula transformation; the upper right corner T represents the transposition; θ yaw Indicates the yaw angle of the first target ring image.

[0064] Step 7-2: Rotate the matrix R after the Rodriguez formula transformation in the camera coordinates r Fuse with the inverse matrix of the rotation matrix R to obtain partial coefficients abc of the plane equation, and return them as well as abc and the camera pose P w2c The product of is the complete coefficients abcd of the plane equation, then:

[0065] P w2c =[R] -1 [Po +(-P)-t CAM ] T (48)

[0066]

[0067] Where P w2c represents the three-dimensional coordinates of the ring contour in the camera coordinate system, P o Represents the three-dimensional coordinates of the center of the target object, represents the first three coefficients of the plane equation in camera coordinates, represents the coefficients of the plane equation;

[0068] Step 7-3: According to the coefficients abcd of the plane equation calculated in step 7-2, the matrix K of the ellipse contour is cnt With depth information P z Perform horizontal splicing to form an augmented matrix P ix ; Use the rotation matrix R and translation vector t CAM And the coefficients abcd of the plane equation, convert the pixel coordinates of the ring contour into world coordinates W t , then the world coordinate W t It is obtained by the following formula:

[0069] P ix =[K cnt P z ] (51)

[0070] P cam =K c -1 [P ix ] T (52)

[0071]

[0072] W t =R(P cam [Z pix ] T )+[P+t CAM ] T (54)

[0073] Where P ix The augmented matrix representing the ellipse contour, K cnt The matrix representing the ellipse contour, P z Represents depth information, P cam Indicates P ix Coordinates in the camera coordinate system, K c represents the camera calibration matrix, [Z pix ] 1The first column represents the depth information, W t Indicates the world coordinates where the drone passes through the red ring outline.

[0074] Step 7-4, W t Transform it into an augmented matrix P by adding a column vector e of all 1s iy , then P iy Perform singular value decomposition to obtain the left singular vector matrix U, the singular value diagonal matrix Σ, and the right singular vector matrix V T Finally, the right singular vector matrix V T The right singular vector of the minimum singular value is taken as W t Fitted plane parameters abcd.

[0075] P iy =[W t e] (55)

[0076] UΣV T =P iy (56)

[0077] Where e represents a column vector of all 1s, W t Indicates the world coordinates of the drone passing through the red circle outline, P iy W t By adding a full 1 column vector to transform the augmented matrix, U represents an m-row by m-column matrix, each column vector of which is The eigenvector of , Σ represents a matrix of m rows and n columns, or The square root of the eigenvalue of is the singular value of the matrix, V T represents an n-row by n-column matrix, where each column vector is The feature vector of .

[0078] Step 7-5: Substitute the unit vector v in the x-axis direction ec1 and the normal vector v of the fitted plane ec2 Multiply the Euclidean norm of orm , by calculating v ec1 and v ec2 The dot product of orm , get the cosine value of the angle between the two vectors cosθ1, and then calculate v ec1 and v ec2 The cross product of ec1 and v ec2 The new vector v of the plane ec3 Finally, the vector v ec3 Normalize it to get the unit vector v in the x-axis directionec1 and the normal vector v of the fitted plane ec2 The rotation vector v' between ec3 , perform arc cosine processing on cosθ1 to obtain the rotation angle θ1.

[0079]

[0080] v ec3 =v ec1 ×v ec2 (60)

[0081]

[0082] where v ec1 Represents a unit vector along the positive direction of the x-axis, and abcd represents the coordinate system W in the current world coordinate system. t Parameters of the fitted plane, v ec2 Indicates extracting the normal vector abc from abcd, v ec3 Indicates perpendicular to v ec1 and v ec2 The new vector of the plane, v' ec3 Represents the cross product vector v ec3 The normalized unit vector, |v ec1 | represents the norm of the unit vector along the x-axis, |v ec2 | represents the norm of the normal vector of the fitted plane, n orm Indicates |v ec1 | and |v ec2 |. cosθ1 represents v ec1 and v ec2 The cosine of the angle between two vectors, v ec3 Indicates perpendicular to v ec1 and v ec2 The new vector of the plane, v' ec3 Represents the vector v ec Normalize to make it a unit vector.

[0083] Step 7-6: Unit vector v' obtained from step 7-5 ec3 The negative value of is converted into the rotation matrix R corresponding to each image through the Rodriguez formula ot , the specific conversion method is obtained by the following formula:

[0084]

[0085] R ot =cosθ1I+(1-cosθ1)[-v' ec3 ][-v' ec3 ] T +sinθ1[-v'ec3 ]^ (63)

[0086] in v Indicates the direction vector, [-v' ec3 ]1 represents the first row of the direction vector, [-v' ec3 ]2 represents the second row of the direction vector, [-v' ec3 ]3 represents the third row of the direction vector, I represents the 3x3 identity matrix, [-v' ec3 ]^ represents the antisymmetric matrix of the direction vector, R ot Represents the rotation matrix after the Rodriguez formula transformation; T in the upper right corner represents transpose; θ1 represents the rotation angle obtained by performing arccosine processing on cosθ1.

[0087] Step 7-7: Fit the three-dimensional point W in the plane t Applied to the rotation matrix R ot , rotate them to the rotation matrix R ot In the coordinate system, we get the rotation matrix R ot The three-dimensional point set P in the coordinate system t , with P t The average value of the x, y and z three-dimensional coordinates of the ring center is used as the initial value of the x, y and z three-dimensional coordinates of the ring center, and the point set P is calculated. ti The distance from the y and z coordinate values ​​of the initial circle center to the y and z coordinate values ​​of the initial circle center is calculated, and the average distance of all points is calculated. Then, the square sum of the difference between the distance of each point to the initial circle center and the average distance is defined as the cost function. Then, the optimal circle center coordinates are fitted by minimizing the cost function. The final radius of the ring is the average distance from the y and z coordinate values ​​of all points to the y and z coordinate values ​​of the optimal circle center.

[0088]

[0089]

[0090] (y c * ,z c * )=argminF(y,z) (70)

[0091]

[0092] Where P t Represented in the rotation matrix R ot A three-dimensional point set in the coordinate system of x i ,y i and z i Each point P in the point set is tiThe x, y and z three-dimensional coordinate values ​​of the point set, n is the number of three-dimensional points in the point set, that is, how many three-dimensional points there are in the point set, x c ,y c and z c are the initial values ​​of the three-dimensional coordinates of the center of the ring x, y and z, R i is every point P in the point set ti The distance between the y and z coordinates of the initial circle center and the y and z coordinates of the initial circle center. F(y,z) is the cost function defined as the sum of the squares of the distance from each point to the initial circle center and the average distance. c * and z c * is the optimal center coordinate fitted by minimizing the cost function, R * The final radius of the ring is the average distance from the y and z coordinates of all points to the y and z coordinates of the optimal center of the circle;

[0093] Steps 7-8, rotate the x, y and z coordinates of the center of the fitted ring back to the world coordinate system. Unit vector v' ec3 The rotation angle θ1 is converted into the rotation matrix R by the Rodriguez formula w The x, y, and z coordinates of the center of the fitted ring are obtained by the rotation matrix R w Get the three-dimensional coordinates P of the center of the circle in the world coordinate system origin , calculate the angle θ of the y and z three-dimensional coordinates of each point on the circle relative to the center of the circle i Finally, we get the angle array θ of the points on the circle. i and the coordinates of the center of the circle P origin .

[0094]

[0095] R w =cosθ1I+(1-cosθ1)v' ec3 [v' ec3 ] T +sinθ1[v' ec3 ]^ (73)

[0096]

[0097]

[0098] Where R w represents the rotation matrix required to transform the x, y, and z coordinates of the fitted ring into the three-dimensional coordinates in the world coordinate system, P origin represents the three-dimensional coordinates of the center of the circle in the world coordinate system, Represents the rotation matrix R w The first row and first column of Represents the rotation matrix R w The first row and second column of Represents the rotation matrix R w The first row and third column of Represents the rotation matrix R w The second row and first column of Represents the rotation matrix R w The second row and second column of Represents the rotation matrix R w The second row and third column of Represents the rotation matrix R w The 3rd row and 1st column of Represents the rotation matrix R w The 3rd row and 2nd column of Represents the rotation matrix R w The third row and third column of i Represents the angle of the y and z three-dimensional coordinates of each point on the circle relative to the center of the circle, x i Represents a three-dimensional point set P t The x coordinate value of the i-th three-dimensional coordinate point in i Represents a three-dimensional point set P t The y coordinate value of the i-th three-dimensional coordinate point in .

[0099] Step 8. Calculate the world coordinates W of the red circle outline through step 7 t Calculate the center, direction vector, yaw angle, tilt angle and radius of the image, update the structure of the image from the motion according to the Rodriguez formula, calculate a new image contour, project it to the image plane, obtain the coordinates in the image coordinate system and the image coordinates, and finally return the corresponding world 3D coordinates, the updated world 3D coordinates W′ t It is obtained by the following formula:

[0100] P pix =K c P w2c T (76)

[0101]

[0102] P z ”=[P pix ] 3 +P z ' (78)

[0104] P 3d =[K cnt P z ” P z ”] (79)

[0105]

[0106] Where P pix Represents the pixel coordinates of the center of the target object, [P pix ] 1,2 represents the x and y coordinates of the pixel coordinates of the center of the target object, [P pix ] 3 The z coordinate of the pixel coordinate representing the center of the target object, P z ' represents the transformed depth vector when the cost function reaches the minimum value, P z " represents the depth value, P 3d Indicated by K cnt The result obtained by multiplying the depth value, plus a matrix composed of the depth value, W′ t Represents the updated world 3D coordinates.

[0107] The UAV path planning method is specifically: the three-dimensional world coordinates of the target ring are calculated by the motion recovery structure center point detection method based on color threshold segmentation, so that the UAV uses this as the end point of the planning when planning the path. For example, using the A* algorithm as the path planning algorithm, it can reach the target point from any starting point with the minimum cost path. The minimum cost path represents the area that the UAV explores through the A* algorithm that is most likely to reach the target ring, thereby minimizing the total cost, reducing the number of nodes that need to be evaluated, improving the overall efficiency of the algorithm, and optimizing the allocation of time resources. If the target cost is set too low, we can still get the shortest path, but the speed will be slower. If the target cost is set too high, we will give up the shortest path, but the A algorithm will run faster. Through the accurate three-dimensional world coordinates of the target ring we provide, the UAV can pass through the target ring more accurately through path planning methods such as the A* algorithm.

[0108] The SfM structure diagram of the system is as follows: Figure 3 As shown. By analyzing different images of the same target (i.e. Figure 3 In Figure 1 , Figure 2 , Figure 3 , Figure 4, Figure 5...) are processed by SfM technology to obtain their world 3D coordinates, and the initial obtained world 3D coordinates are corrected to improve accuracy and robustness.

[0109] The motion recovery structure center point detection method based on color threshold segmentation in the present invention adopts the motion recovery structure to extract key information such as the three-dimensional coordinates of the center of the outer ring, the direction vector and the yaw angle. After a series of iterative optimizations, the three-dimensional coordinates of the center of the ring are corrected more accurately. This provides a more accurate basis for subsequent UAV path planning, ensuring that the UAV can maintain stable control when crossing the ring. By pre-excluding the ring contour images that do not meet the conditions, more accurate and robust three-dimensional coordinates of the center of the ring are provided for subsequent UAV path planning.

[0110] For example, the target three-dimensional coordinates obtained from the drone's sensor are (23.211326599121094, 6.285023212432861, -0.5427668690681458). After a series of iterative optimizations, the target three-dimensional coordinates are updated to (23.20131226 6.18176827 -0.93959644), (23.200723746.15992615 -0.94079561). Finally, the target three-dimensional coordinates are (23.200117396.13685448 -0.92659268). At this time, the drone can just cross the target circle.

[0111] In addition, it will be appreciated by those skilled in the art that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0112] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0113] These computer program instructions may also be stored in a computer readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture including an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0114] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process in the computer or other programmable device. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0115] The above-mentioned embodiments of the present invention are only used to explain the calculation model and calculation process of the present invention in detail, and are not the only limitation to the implementation of the present invention. For those of ordinary skill in the art, various forms of changes or adjustments can be made on the basis of the above description. It is impossible to exhaust all possible implementation methods here, so the obvious changes or adjustments caused by the technical solution of the present invention should still be regarded as belonging to the protection scope of the present invention. This means that any technically reasonable and obvious modification can be regarded as belonging to the protection scope of the present invention, and is not limited to the specific embodiments in the above detailed description.

Claims

1. A method for detecting center points of motion-recovered structures based on color threshold segmentation, characterized in that: The method comprises: Step 1: Color Threshold Segmentation 1-1 Camera calibration: Through the camera calibration matrix, the mutual conversion between the world coordinate system and the pixel coordinate system is realized; 1-2 Get RGB image: Get the RGB ring image information from the camera, including the image's rotation matrix, image color information, and pose information; if the number of contour points of the camera's RGB ring image is less than 10, skip the current image and do not perform subsequent ellipse fitting; 1-3 Get the ellipse outline: Based on the red threshold, the corresponding elliptical contour of the ring image is obtained, and the array data, rotation matrix information, posture information, and target data of the multi-channel RGB ring image are extracted; Step 2: Preprocessing of target image 2-1 Projection transformation: Describe the outer contour of the ring in 3D space; the specific process is as follows: Project the contour points of the RGB ring image in steps 1-3 onto the image plane of the camera through the camera's projection matrix to obtain the corresponding two-dimensional image coordinates. Then, create three image copies based on the two-dimensional image coordinates, and use green, yellow, and blue colors to depict the image contours on these image copies respectively; 2-2 Image selection: Screen the images processed in step 2-1 to remove incomplete images and images with multiple similar targets; 2-3 Pre-providing of ring information: providing the approximate 3D coordinates, direction vector and yaw angle of the center point of the target ring image, and using them as the system's starting 3D coordinates, direction vector and yaw angle, so as to be fused with other 3D coordinates of the center point obtained from the same ring image, to obtain the 3D coordinates of the center point with smaller error; Step 3: Structure from Motion 3-1 Calculate the parameters of the fitting graph: Based on the three-dimensional image coordinates of the target ring image in step 2, its position and orientation in space are calculated; first, a plane containing the ring is fitted, and the rotation vector and yaw angle required to rotate the plane to be parallel to the x-axis are determined; then, using the Rodriguez rotation formula, these rotation parameters are applied to the three-dimensional point set and transferred to a new coordinate system, in which a circle is fitted to the point set to determine the center and radius of the circle; finally, the world coordinates of the center of the ring, the rotation vector, the yaw angle, and the radius of the ring are calculated; 3-2 Update SfM: Use the Rodriguez formula to calculate the direction vector and yaw angle information of the new center of the same target ring to obtain more accurate ring information, update the three-dimensional coordinates of the image, map the world coordinates to the camera coordinates, and finally return the updated world coordinates; Step 4: Iterate the 3D image center point of the target ring 4-1 Iterate the three-dimensional coordinates of the center of the circle: Iterate the three-dimensional image center point of the target circle through steps 1 to 3, analyze multiple three-dimensional coordinates of the center of the same target circle, direction vectors, and yaw angles, and obtain more accurate three-dimensional coordinates of the center of the circle.

2. The method for detecting center points of motion-recovered structures based on color threshold segmentation according to claim 1, characterized in that: In step 1-1, first set a rotation matrix R from world coordinates to camera coordinates w2c and the camera intrinsic parameter K CAM , the two are multiplied by dot product to obtain the camera intrinsic parameter matrix K ck .

3. The method for detecting center points of motion-recovered structures based on color threshold segmentation according to claim 2, characterized in that: In the step 1-3, the red channel pixel value of the ellipse in the ring image is extracted for threshold processing, and the pixel values ​​of the pixels that meet the range between 220 and 240, that is, the pixel values ​​of the high-brightness red area are set to 128, and the other pixels that do not meet this range are set to 0, so as to generate a binary image; Find contours on a binary image and return a list of contours α cnts and the hierarchy α hierarchy , hierarchy α hierarchy Contains index information of upper and lower contours, ellipse contour α cnt Indicates traversal of the contour list α cnts For each contour in , if the number of points of the contour is greater than a preset threshold, the points of the contour are fitted into an ellipse, and the ellipse information is obtained at the same time, wherein the ellipse information includes the center coordinates of the ellipse, the lengths of the major axis and the minor axis, and the rotation angle of the ellipse; Add ellipse information to ellipse list β ellps In the example, the elliptical contour α that meets the requirements is cnt Information added to profile list β cnt_ret Finally, the binary image t after threshold processing is obtained photo , contour list β cnt_ret and the ellipse list β ellps .

4. The method for detecting center points of motion-recovered structures based on color threshold segmentation according to claim 3, characterized in that: In step 2-1, the three-dimensional point W in the world coordinate system t Through the camera projection matrix K Rt Project it onto the image plane of the camera to obtain the corresponding two-dimensional image coordinates P t2d , the specific process is: Step 2-1-1: The rotation matrix R and translation vector t of each frame of RGB ring image taken by the drone camera CAM Merge into the projection matrix, and then the camera intrinsic parameter matrix K ck Multiply it with the combined projection matrix to get the projection matrix K of the ideal camera Rt : t CAM =R -1 [-(P+T CAM )] (1) T proj ={R -1 ,t CAM } (2) K Rt =K ck ·T proj (3) where t CAM represents the camera's translation vector, R represents the camera's rotation matrix, P represents the camera's current position, and T CAM Represents the three-dimensional coordinates of the camera. The superscript -1 represents the inverse of the matrix. proj The matrix representing the camera rotation and translation, K Rt represents the camera projection matrix, K ck Represents the camera calibration matrix; Step 2-1-2: The three-dimensional coordinates of the target object are passed through the camera's projection matrix K Rt Projection is performed, and finally the x and y coordinates of the projected three-dimensional coordinates of the target object are divided by the z coordinate to obtain the two-dimensional image coordinates. The specific two-dimensional image coordinate conversion is obtained by the following formula: P 3d =K Rt ·[P wt ] T (4) Where P wt represents the three-dimensional coordinates of the target object, P 3d represents the three-dimensional coordinates of the ring to be traversed after the transformation, and the superscript T represents the transpose of the matrix. 3d ] 1,2 Indicates P 3d The x and y coordinates, [P 3d ]3 means P 3d The z coordinate, P t2d Represents the two-dimensional image coordinates obtained by projecting the target object onto the image plane of the camera.

5. The method for detecting center points of motion-recovered structures based on color threshold segmentation according to claim 4, characterized in that: The process of step 3-1 is: according to the three-dimensional coordinates of the approximate center point of the target ring image, the direction vector and the yaw angle, the Rodriguez formula is used to convert them into the rotation matrix R r , calculate the coefficients abcd of the plane equation in camera coordinates, and transform the image coordinates of the image into world coordinates W t ; Specifically include: Step 3-1-1, according to step 2-3, get the direction vector v of the starting target ring image ec , converted to the rotation matrix R through the Rodriguez formula r , the specific conversion method is obtained by the following formula: R r =cosθ yaw I+(1-cosθ yaw )[v ec ][v ec ] T +sinθ yaw [v ec ]^ (7) where v ec Represents the direction vector of the first target ring image, [v ec ]1 represents the first row of direction vectors, [v ec ]2 represents the second row of direction vectors, [v ec ]3 represents the third row of the direction vector, I represents the 3x3 identity matrix, [v ec ]^ represents the antisymmetric matrix of the direction vector, R r represents the rotation matrix after the Rodriguez formula transformation; the upper right corner T represents the transposition; θ yaw Indicates the yaw angle of the first target ring image; Step 3-1-2: Rotate the matrix R after the Rodriguez formula transformation in the camera coordinates r The partial coefficients abc of the plane equation are obtained by fusing with the inverse matrix of the rotation matrix R, and the partial coefficients abc of the plane equation and the relationship between abc and the camera posture P are returned. w2c The product of is the complete coefficients abcd of the plane equation, then: P w2c =[R] -1 [P o +(-P)-t CAM ] T (8) Where P w2c represents the three-dimensional coordinates of the ring contour in the camera coordinate system, P o Represents the three-dimensional coordinates of the center of the target object, represents the first three coefficients of the plane equation in camera coordinates, represents the coefficients of the plane equation; Step 3-1-3: According to the coefficients abcd of the plane equation calculated in step 3-1-2, the matrix K of the ellipse contour is cnt With depth information P z Perform horizontal splicing to form an augmented matrix P ix ; Use the rotation matrix R and translation vector t CAM And the coefficients abcd of the plane equation, convert the pixel coordinates of the ring contour into world coordinates W t , then the world coordinate W t It is obtained by the following formula: P ix =[K cnt P z ] (11) W t =R(P cam [Z pix ] T )+[P+t CAM ] T (14) Where P ix The augmented matrix representing the ellipse contour, K cnt The matrix representing the ellipse contour, P z Represents depth information, P cam Indicates P ix Coordinates in the camera coordinate system, K c represents the camera calibration matrix, [Z pix ] 1 The first column represents the depth information, W t Indicates the world coordinates where the drone passes through the red ring outline; Step 3-1-4, W t Transform it into an augmented matrix P by adding a column vector e of all 1s iy , then P iy Perform singular value decomposition to obtain the left singular vector matrix U, the singular value diagonal matrix Σ, and the right singular vector matrix V T ; Finally, the right singular vector matrix V T The right singular vector of the minimum singular value is taken as W t Fitted plane parameters abcd; Q iy =[W t e] (15) UΣV T =P iy (16) Where e represents a column vector of all 1s, W t Indicates the world coordinates of the drone passing through the red circle outline, P iy W t By adding a full 1 column vector to transform the augmented matrix, U represents an m-row by m-column matrix, each column vector of which is The eigenvector of , Σ represents a matrix of m rows and n columns, or The square root of the eigenvalue of is the singular value of the matrix, V T represents an n-row by n-column matrix, where each column vector is The eigenvector of Step 3-1-5: Substitute the unit vector v in the x-axis direction ec1 and the normal vector v of the fitted plane ec2 Multiply the Euclidean norm of orm , by calculating v ec1 and v ec2 The dot product of orm , get the cosine value of the angle between the two vectors cosθ1, and then calculate v ec1 and v ec2 The cross product of ec1 and v ec2 The new vector v of the plane ec3 , and finally the vector v ec3 Normalize it to get the unit vector v in the x-axis direction ec1 and the normal vector v of the fitted plane ec2 The rotation vector v' between ec3 , perform arccosine processing on cosθ1 to obtain the rotation angle θ1; n orm =|v ec1 ||v ec2 | (18) v ec3 =v ec1 ×v ec2 (20) where v ec1 Represents a unit vector along the positive direction of the x-axis, and abcd represents the coordinate system W in the current world coordinate system. t Parameters of the fitted plane, v ec2 Indicates extracting the normal vector abc from abcd, v ec3 Indicates perpendicular to v ec1 and v ec2 The new vector of the plane, v' ec3 represents the cross product vector v ec3 The normalized unit vector, |v ec1 | represents the norm of the unit vector along the x-axis, |v ec2 | represents the norm of the normal vector of the fitted plane, n orm Indicates |v ec1 | and |v ec2 |, cosθ1 represents v ec1 and v ec2 The cosine of the angle between two vectors, v ec3 Indicates perpendicular to v ec1 and v ec2 The new vector of the plane, v' ec3 Represents the vector v ec Normalize it to make it a unit vector; Step 3-1-6: Unit vector v' obtained from step 3-1-5 ec3 The negative value of is converted into the rotation matrix R corresponding to each image through the Rodriguez formula ot , the specific conversion method is obtained by the following formula: R ot =cosθ1I+(1-cosθ1)[-v' ec3 ][-v' ec3 ] T +sinθ1[-v' ec3 ]^ (23) in v Indicates the direction vector, [-v' ec3 ]1 represents the first row of the direction vector, [-v' ec3 ]2 represents the second row of the direction vector, [-v' ec3 ]3 represents the third row of the direction vector, I represents the 3x3 identity matrix, [-v' ec3 ]^ represents the antisymmetric matrix of the direction vector, R ot represents the rotation matrix after the Rodriguez formula transformation; the upper right corner T represents the transpose; θ1 represents the rotation angle obtained by performing arccosine processing on cosθ1; Step 3-1-7: Fit the three-dimensional point W in the plane t Applied to the rotation matrix R ot , rotate them to the rotation matrix R ot In the coordinate system, we get the rotation matrix R ot The three-dimensional point set P in the coordinate system t , with P t The average value of the x, y and z three-dimensional coordinates of the ring center is used as the initial value of the x, y and z three-dimensional coordinates of the ring center, and the point set P is calculated. ti The distance from the y and z coordinate values ​​of the initial circle center to the y and z coordinate values ​​of the initial circle center is calculated, and the average distance of all points is calculated. Then, the square sum of the difference between the distance of each point to the initial circle center and the average distance is defined as the cost function. Then, the optimal circle center coordinates are fitted by minimizing the cost function. The final radius of the ring is the average distance from the y and z coordinate values ​​of all points to the y and z coordinate values ​​of the optimal circle center. P t =[R ot [W t ] T ] T (24) Where P t Represented in the rotation matrix R ot A three-dimensional point set in the coordinate system of x i ,y i and z i Each point P in the point set is ti The x, y and z three-dimensional coordinate values ​​of the point set, n is the number of three-dimensional points in the point set, that is, how many three-dimensional points there are in the point set, x c ,y c and z c are the initial values ​​of the three-dimensional coordinates of the center of the ring x, y and z, R i is every point P in the point set ti The distance between the y and z coordinates of the initial circle center and the y and z coordinates of the initial circle center. F(y,z) is the cost function, which is defined as the sum of the squares of the difference between the distance from each point to the initial circle center and the average distance. and is the optimal center coordinate fitted by minimizing the cost function, R * The final radius of the ring is the average distance from the y and z coordinates of all points to the y and z coordinates of the optimal center of the circle; Step 3-1-8, rotate the x, y and z three-dimensional coordinates of the center of the fitted ring back to the world coordinate system; unit vector v' ec3 The rotation angle θ1 is converted into the rotation matrix R by the Rodriguez formula w The x, y, and z coordinates of the center of the fitted ring are obtained by the rotation matrix R w Get the three-dimensional coordinates P of the center of the circle in the world coordinate system origin , calculate the angle θ of the y and z three-dimensional coordinates of each point on the circle relative to the center of the circle i , and finally get the angle array θ of the points on the circle i and the coordinates of the center of the circle P origin ; R w =cosθ1I+(1-cosθ1)v' ec3 [v' ec3 ] T +sinθ1[v' ec3 ]^ (33) Where R w represents the rotation matrix required to transform the x, y, and z coordinates of the fitted ring into the three-dimensional coordinates in the world coordinate system, P origin represents the three-dimensional coordinates of the center of the circle in the world coordinate system, Represents the rotation matrix R w The first row and first column of Represents the rotation matrix R w The first row and second column of Represents the rotation matrix R w The first row and third column of Represents the rotation matrix R w The second row and first column of Represents the rotation matrix R w The second row and second column of Represents the rotation matrix R w The second row and third column of Represents the rotation matrix R w The 3rd row and 1st column of Represents the rotation matrix R w The 3rd row and 2nd column of Represents the rotation matrix R w The third row and third column of i Represents the angle of the y and z three-dimensional coordinates of each point on the circle relative to the center of the circle, x i Represents a three-dimensional point set P t The x coordinate value of the i-th three-dimensional coordinate point in i Represents a three-dimensional point set P t The y coordinate value of the i-th three-dimensional coordinate point in .

6. The method for detecting center points of motion-recovered structures based on color threshold segmentation according to claim 5, characterized in that: The specific process of step 3-2 is as follows: The world coordinates W of the red circle outline are calculated through step 3-1 t Calculate the center, direction vector, yaw angle, tilt angle and radius of the image, update the structure of the image from the motion according to the Rodriguez formula, calculate a new image contour, project it to the image plane, obtain the coordinates in the image coordinate system and the image coordinates, and finally return the corresponding world 3D coordinates, the updated world 3D coordinates W' t It is obtained by the following formula: P” z =[P pix ] 3 +P z ' (38) P 3d =[K cnt P” z P” z ] (39) W’ t =R[K c ] -1 [P 3d ] T +(P+t CAM ) (40) Where P pix Represents the pixel coordinates of the center of the target object, [P pix ] 1,2 represents the x and y coordinates of the pixel coordinates of the center of the target object, [P pix ] 3 The z coordinate of the pixel coordinate representing the center of the target object, P z ' represents the transformed depth vector when the cost function reaches the minimum value, P" z Indicates the depth value, P 3d Indicated by K cnt The result obtained by multiplying the depth value, plus a matrix composed of the depth value, W t 'Indicates the updated world 3D coordinates.

7. A UAV path planning method, characterized in that: The method comprises the following steps: calculating the three-dimensional world coordinates of the target circle by the motion recovery structure center point detection method based on color threshold segmentation as described in any one of claims 1 to 6, so that the three-dimensional world coordinates of the target circle are used as the planning end point when the drone plans the path.

8. A motion recovery structure center point detection system based on color threshold segmentation, characterized in that: Used to implement the method according to any one of claims 1 to 6.

9. An electronic device, characterized in that: include: one or more processors; A memory for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the method according to any one of claims 1 to 7.

10. A computer-readable storage medium storing computer-executable instructions, wherein the instructions are used to implement the method according to any one of claims 1 to 7 when executed.

Images