Rotation estimation method, system and device based on ball pole projection and storage medium

Abstract

The invention discloses a rotation estimation method, system and device based on spherical pole projection and a storage medium, and the method comprises the steps: carrying out the decoupling of a target rotation space, and obtaining a rotation axis space and a rotation angle space; the problem of calculating the rotating shaft is converted into the problem of finding the maximum intersection points of the circular ring on the three-dimensional spherical surface; converting the problem of searching for the maximum intersection points of the circular ring on the three-dimensional spherical surface into the problem of searching for the maximum intersection points of the circular ring on the two-dimensional plane based on the ball pole projection simplified operation; calculating a rotation axis based on spatial voting, and determining an optimal rotation axis or a plurality of target rotation axes; determining an optimal rotation angle corresponding to the optimal rotation axis or a plurality of target rotation angles corresponding to the plurality of target rotation axes; and determining an optimal rotation estimation according to the optimal rotation axis and the optimal rotation angle, or determining a plurality of rotation estimations according to the target rotation axis and the corresponding target rotation angle. According to the method, high-efficiency and high-robustness rotation estimation is realized, and the method can be applied to estimation of multiple rotations at the same time and can be applied to the technical field of computer vision.

Description

Technical Field

[0001] The present invention relates to the technical field of computer vision, and in particular to a rotation estimation method, system, device and storage medium based on stereographic projection. Background Art

[0002] Rotation estimation is a core problem in the fields of computer vision and robotics. In this problem, given two sets of 3D point clouds, the task is to achieve the optimal alignment effect of these two sets of point clouds by estimating the best rotation between them. In addition, the simultaneous estimation of multiple rotations between two sets of 3D point clouds is also widely required in many important applications, such as autonomous driving, robot perception and navigation, etc.

[0003] Due to the non-convexity of the rotation space the task of solving rotation estimation becomes a typical non-convex optimization problem, and the complexity of solving non-convex optimization problems is extremely high, belonging to the NP-hard problem (NP-Hard Problem). Specifically, the rotation estimation problem faces two main challenges: (1) the robustness problem, that is, how to ensure that the obtained solution is the global optimal solution; (2) the low efficiency problem, that is, how to solve the optimal rotation estimation problem in real time. Obviously, these two problems are crucial in rotation estimation. For example, in the field of autonomous driving, low accuracy and poor efficiency of rotation estimation may both lead to serious traffic accidents and threaten people's lives. Therefore, numerous studies are dedicated to solving these two problems, and also hope to find methods that can simultaneously solve multiple rotation estimations to meet the needs of more practical applications.

[0004] The existing rotation estimation methods mainly focus on the following five types:

[0005] 1) Optimization-based algorithms. For example, the ICP (Iterative Closest Point) algorithm, which relies on the gradient of the objective function. When using appropriate initial values, these methods can achieve high accuracy. Although the computational efficiency of such algorithms is very high, due to non-convexity, they may converge to local optima. Therefore, they are usually used as the final optimization step in various algorithm frameworks.

[0006] 2) Heuristic-based algorithms. For example, the FGR (Fast Global Registration) algorithm, which can avoid falling into local optima, but can only obtain the optimal solution with a certain probability. Specifically, this type of algorithm cannot theoretically guarantee the optimality of the solution.

[0007] 3) Algorithms based on the Branch and Bound framework. Such algorithms systematically search the solution space until the global optimal solution is found, but the search time is too long and not suitable for application in real-time systems.

[0008] 4) Algorithms based on deep learning. Although this algorithm performs well in terms of robustness and efficiency, a large amount of data needs to be trained in the preparatory work.

[0009] 5) Multi-model fitting algorithms for solving multiple rotation estimations. Traditional methods such as the SequentialRANSAC algorithm adopt a sequential processing method. This method first uses the RANSAC algorithm to fit a model, then removes all correctly matched points (inliers) in the currently selected model, and then repeats this process until the stopping condition is reached, thereby successively fitting multiple models. Although RANSAC itself is robust to outliers, this sequential processing may cause the outliers of the previous model to affect the subsequent model estimation. Inaccurate initial model estimations may also cause errors to accumulate in subsequent models. In other words, there is currently no algorithm that can simultaneously obtain multiple rotation estimations.

[0010] In summary, the existing rotation estimation methods cannot balance the accuracy in finding corresponding relationships and the efficiency in processing large-scale data. In addition, they cannot achieve simultaneous estimation of multiple rotations. Summary of the Invention

[0011] An object of the present invention is to solve at least to a certain extent one of the technical problems existing in the prior art.

[0012] To this end, an object of an embodiment of the present invention is to provide a rotation estimation method based on stereographic projection, which realizes high-efficiency and high-robustness rotation estimation and can be simultaneously applied to the estimation of multiple rotations.

[0013] Another object of an embodiment of the present invention is to provide a rotation estimation system based on stereographic projection.

[0014] In order to achieve the above technical objectives, the technical solutions adopted in the embodiments of the present invention include:

[0015] In a first aspect, an embodiment of the present invention provides a rotation estimation method based on stereographic projection, including the following steps:

[0016] Decouple the target rotation space to obtain a rotation axis space and a rotation angle space;

[0017] Establish a geometric constraint containing only the rotation axis, and transform the problem of calculating the rotation axis into the problem of finding the most intersections of circles on a three-dimensional sphere;

[0018] Simplify the operation based on stereographic projection, and transform the problem of finding the most intersections of circles on a three-dimensional sphere into the problem of finding the most intersections of circles on a two-dimensional plane;

[0019] Calculate the rotation axis based on spatial voting, and determine the optimal rotation axis or multiple target rotation axes according to the peaks obtained from the voting;

[0020] Calculate the rotation angle based on spatial voting, and determine the optimal rotation angle corresponding to the optimal rotation axis or multiple target rotation angles corresponding to the multiple target rotation axes according to the peaks obtained from the voting;

[0021] Determine the optimal rotation estimation of the observed point cloud and the target point cloud according to the optimal rotation axis and the optimal rotation angle, or determine multiple rotation estimations of the observed point cloud and the target point cloud according to the multiple target rotation axes and the corresponding multiple target rotation angles.

[0022] Further, in an embodiment of the present invention, the geometric constraint is:

[0023] ∠(r,x - y) = 90°

[0024] where r represents the rotation axis to be solved, x represents the observed point cloud, and y represents the target point cloud;

[0025] The problem of transforming the calculation of the rotation axis into the problem of finding the most intersections of circles on a three - dimensional sphere specifically includes:

[0026] For any given vector point on the three - dimensional sphere, the rotation axis that satisfies the geometric constraint is perpendicular to it, so as to determine that the rotation axis that satisfies the geometric constraint is within a space circle;

[0027] For multiple given vector points on the three - dimensional sphere, the rotation axes that satisfy the geometric constraint are simultaneously within multiple space circles, thus transforming the problem of calculating the rotation axis into the problem of determining the orientation of the rotation axis to be solved according to the intersections of multiple space circles on the three - dimensional sphere.

[0028] Further, in an embodiment of the present invention, the operation is simplified based on stereographic projection, and the problem of finding the most intersections of circles on a three - dimensional sphere is transformed into the problem of finding the most intersections of circles on a two - dimensional plane, which specifically includes:

[0029] Use stereographic projection to project multiple space circles on the three - dimensional sphere onto a two - dimensional plane to obtain multiple plane circles on the corresponding two - dimensional plane;

[0030] The problem of determining the orientation of the rotation axis to be solved according to the intersections of multiple space circles on the three - dimensional sphere is transformed into the problem of determining the orientation of the rotation axis to be solved according to the intersections of multiple plane circles on the two - dimensional plane.

[0031] Further, in an embodiment of the present invention, the calculation of the rotation axis based on spatial voting, and determining the optimal rotation axis or multiple target rotation axes according to the peaks obtained from the voting specifically includes:

[0032] Discretize the solution space of the projection of the two-dimensional rotation axis with a grid, and count the number of circular rings passing through each grid area based on spatial voting statistics;

[0033] Determine several of the grid areas with the most circular rings passing through according to the peaks obtained by voting as the target grid areas, determine the position where the rotation axis to be solved is mapped to the two-dimensional plane according to the target grid areas, and then obtain several of the optimal rotation axes on the three-dimensional sphere by using the inverse operation of stereographic projection.

[0034] Further, in an embodiment of the present invention, the rotation angle is calculated based on spatial voting, and the optimal rotation angle corresponding to the optimal rotation axis or the multiple target rotation angles corresponding to the multiple target rotation axes is determined according to the peaks obtained by voting, which specifically includes:

[0035] Determine the trigonometric function relationship of the corresponding optimal rotation angle / target rotation angle according to the optimal rotation axis / target rotation axis and the Rodriguez rotation formula;

[0036] Substitute the observed point cloud and the target point cloud as input data into the trigonometric function relationship to obtain multiple alternative rotation angles corresponding to the optimal rotation axis / target rotation axis;

[0037] Based on spatial voting statistics, count the number of input data corresponding to each alternative rotation angle, and determine the alternative rotation angle corresponding to the most input data according to the peaks obtained by voting as the optimal rotation angle / target rotation angle corresponding to the optimal rotation axis / target rotation axis.

[0038] Further, in an embodiment of the present invention, the optimal rotation estimation of the observed point cloud and the target point cloud is determined according to the optimal rotation axis and the optimal rotation angle, which specifically is:

[0039] Substitute the optimal rotation axis and the optimal rotation angle into the Rodriguez rotation formula to obtain the optimal rotation estimation of the observed point cloud and the target point cloud.

[0040] Further, in an embodiment of the present invention, the multiple rotation estimations of the observed point cloud and the target point cloud are determined according to the multiple target rotation axes and the corresponding multiple target rotation angles, which specifically is:

[0041] Substitute the determined target rotation axis and the corresponding target rotation angle into the Rodriguez rotation formula to obtain multiple rotation estimations of the observed point cloud and the target point cloud.

[0042] In a second aspect, an embodiment of the present invention provides a rotation estimation system based on stereographic projection, including:

[0043] A rotation space decoupling module, configured to decouple a target rotation space to obtain a rotation axis space and a rotation angle space;

[0044] A first transformation module, configured to establish geometric constraints containing only rotation axes, and transform the problem of calculating rotation axes into the problem of finding the most intersection points of a circular ring on a three-dimensional sphere;

[0045] A second transformation module, configured to simplify the operation based on stereographic projection, and transform the problem of finding the most intersection points of a circular ring on a three-dimensional sphere into the problem of finding the most intersection points of a circular ring on a two-dimensional plane;

[0046] A rotation axis solving module, configured to calculate a rotation axis based on spatial voting, and determine an optimal rotation axis or multiple target rotation axes according to the peak value obtained by voting;

[0047] A rotation angle solving module, configured to calculate a rotation angle based on spatial voting, and determine an optimal rotation angle corresponding to the optimal rotation axis or multiple target rotation angles corresponding to the multiple target rotation axes according to the peak value obtained by voting;

[0048] A rotation estimation module, configured to determine an optimal rotation estimation of an observation point cloud and a target point cloud according to the optimal rotation axis and the optimal rotation angle, or determine multiple rotation estimations of the observation point cloud and the target point cloud according to the multiple target rotation axes and the corresponding multiple target rotation angles.

[0049] In a third aspect, an embodiment of the present invention provides a rotation estimation device based on stereographic projection, including:

[0050] At least one processor;

[0051] At least one memory, configured to store at least one program;

[0052] When the at least one program is executed by the at least one processor, the at least one processor is caused to implement the above-mentioned rotation estimation method based on stereographic projection.

[0053] In a fourth aspect, an embodiment of the present invention further provides a computer-readable storage medium, in which a program executable by a processor is stored, and the program executable by the processor is used to execute the above-mentioned rotation estimation method based on stereographic projection when executed by the processor.

[0054] The advantages and beneficial effects of the present invention will be partially given in the following description, partially become obvious from the following description, or be understood through the practice of the present invention:

[0055] In the embodiments of the present invention, the target rotation space is decoupled to obtain the rotation axis space and the rotation angle space. Geometric constraints containing only the rotation axis are established, and the problem of calculating the rotation axis is transformed into the problem of finding the maximum number of intersection points of a circular ring on a three-dimensional sphere. Based on stereographic projection, the operation is simplified, and the problem of finding the maximum number of intersection points of a circular ring on a three-dimensional sphere is transformed into the problem of finding the maximum number of intersection points of a circular ring on a two-dimensional plane. The rotation axis is calculated based on spatial voting, and the optimal rotation axis or multiple target rotation axes are determined according to the peak value obtained from the voting. The rotation angle is calculated based on spatial voting, and the optimal rotation angle corresponding to the optimal rotation axis or multiple target rotation angles corresponding to multiple target rotation axes are determined according to the peak value obtained from the voting. The optimal rotation estimation of the observed point cloud and the target point cloud is determined according to the optimal rotation axis and the optimal rotation angle, or multiple rotation estimations of the observed point cloud and the target point cloud are determined according to multiple target rotation axes and the corresponding multiple target rotation angles. In the embodiments of the present invention, the problem of solving the rotation estimation is first decoupled into the problems of calculating the rotation axis and the rotation angle, and then geometric constraints containing only the rotation axis are established. The problem of calculating the rotation axis is transformed into the problem of finding the maximum number of intersection points of a circular ring on a three-dimensional sphere, and then based on stereographic projection, the operation is simplified. The problem of finding the maximum number of intersection points of a circular ring on a three-dimensional sphere is transformed into the problem of finding the maximum number of intersection points of a circular ring on a two-dimensional plane. Furthermore, stereographic projection and spatial voting techniques are used to solve the rotation axis and the rotation angle, achieving high-efficiency and high-robustness rotation estimation, and it can be applied to the estimation of multiple rotations simultaneously. In addition, the embodiments of the present invention have high robustness when dealing with the rotation estimation problems of large-scale data (10 5 ) and a large number of outliers (90%), and can be applied to real-time systems. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following introduces the drawings required to be used in the embodiments of the present invention. It should be understood that the drawings introduced below are only for conveniently and clearly presenting some embodiments of the technical solutions in the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative efforts.

[0057] Figure 1 It is a flowchart of the steps of a rotation estimation method based on stereographic projection provided by the embodiments of the present invention;

[0058] Figure 2 It is a schematic diagram of rotational motion provided by the embodiments of the present invention;

[0059] Figure 3 It is a schematic diagram of the geometric structure regarding the rotation axis provided by the embodiments of the present invention;

[0060] Figure 4 It is a schematic diagram of stereographic projection provided by the embodiments of the present invention;

[0061] Figure 5 Schematic diagram of a circular ring and its intersection points on a two-dimensional plane provided by an embodiment of the present invention;

[0062] Figure 6 Schematic diagram of solving for the optimal rotation axis based on spatial voting provided by an embodiment of the present invention;

[0063] Figure 7 Schematic diagram of solving for multiple target rotation axes based on spatial voting provided by an embodiment of the present invention;

[0064] Figure 8 Block diagram of a rotation estimation system based on stereographic projection provided by an embodiment of the present invention;

[0065] Figure 9 Block diagram of a rotation estimation device based on stereographic projection provided by an embodiment of the present invention.

[0066] Reference numerals: 1, Observation point cloud; 2, Rotation angle; 3, Target point cloud; 4, Rotation axis; 5, Origin; 6, Point on the unit sphere; 7, Circular ring on the unit sphere representing the direction of the rotation axis; 8, Center of the unit sphere; 9, Intersection points of the circular rings on the unit sphere; 10, North pole point on the unit sphere; 11, Intersection points of the circular rings on the two-dimensional plane after stereographic projection; 12, Circular ring on the two-dimensional plane after stereographic projection; 13, Circular ring on the unit sphere; 14, Rotation axis found through spatial voting on the two-dimensional plane. Detailed implementation manners

[0067] The following details the embodiments of the present invention. The examples of the embodiments are shown in the accompanying drawings, where the same or similar reference numerals throughout denote the same or similar elements or elements having the same or similar functions. The embodiments described below by referring to the accompanying drawings are exemplary and are only used to explain the present invention and should not be construed as a limitation to the present invention. For the step numbers in the following embodiments, they are only set for the convenience of elaboration and explanation, and no limitation is imposed on the order between the steps. The execution order of each step in the embodiments can be adaptively adjusted according to the understanding of those skilled in the art.

[0068] In the description of the present invention, "a plurality of" means two or more. If there is a description of the first and the second, it is only for the purpose of distinguishing technical features and should not be construed as indicating or implying relative importance or implicitly indicating the quantity of the indicated technical features or implicitly indicating the sequence of the indicated technical features. In addition, unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the technical field to which this application belongs.

[0069] Referring to Figure 1 , an embodiment of the present invention provides a rotation estimation method based on stereographic projection, which specifically includes the following steps:

[0070] S101. Decouple the target rotation space to obtain a rotation axis space and a rotation angle space;

[0071] S102. Establish geometric constraints containing only rotation axes, and transform the problem of calculating rotation axes into the problem of finding the maximum number of intersection points of a circular ring on a three-dimensional sphere;

[0072] S103. Simplify the operation based on stereographic projection, and transform the problem of finding the maximum number of intersection points of a circular ring on a three-dimensional sphere into the problem of finding the maximum number of intersection points of a circular ring on a two-dimensional plane;

[0073] S104. Calculate the rotation axis based on spatial voting, and determine the optimal rotation axis or multiple target rotation axes according to the peak value obtained by voting;

[0074] S105. Calculate the rotation angle based on spatial voting, and determine the optimal rotation angle corresponding to the optimal rotation axis or multiple target rotation angles corresponding to multiple target rotation axes according to the peak value obtained by voting;

[0075] S106. Determine the optimal rotation estimation of the observed point cloud and the target point cloud according to the optimal rotation axis and the optimal rotation angle, or determine multiple rotation estimations of the observed point cloud and the target point cloud according to multiple target rotation axes and the corresponding multiple target rotation angles.

[0076] Specifically, decouple the rotation space, and decouple the rotation space into a rotation axis space and a rotation angle space [0, π] to reduce the time complexity; establish geometric constraints containing only rotation axes, and transform the problem of calculating rotation axes into the problem of finding the maximum number of intersection points of a circular ring on a unit sphere; simplify the operation by stereographic projection, and transform the problem of finding the maximum number of intersection points of a circular ring in a three-dimensional space into the problem of finding the maximum number of intersection points of a circular ring on a two-dimensional plane; calculate the rotation axis by spatial voting, and the peak value obtained by voting is the optimal rotation axis. If there are multiple peak values, multiple rotation axes can be found simultaneously; calculate the rotation angle by spatial voting, and the peak value obtained by voting is the optimal rotation angle. If there are multiple peak values, multiple rotation angles can be found simultaneously; achieve an optimal rotation estimation, and use the optimal rotation axis and the optimal rotation angle obtained by calculation to solve the optimal rotation estimation by using the Rodriguez formula; simultaneously achieve multiple rotation estimations, and use the multiple rotation axes and rotation angles obtained by calculation to simultaneously solve multiple rotation estimations by using the Rodriguez formula.

[0077] Regarding the robustness of rotation estimation, in the embodiments of the present invention, a robust objective function is established to suppress the influence brought by outliers, and the maximum consensus set is used to construct the objective function as follows:

[0078]

[0079] Among them, is an indicator function. When the condition inside is true, it returns 1; otherwise, it returns 0. ||·|| is the Euler norm, and ε is a threshold. If the distance between two points is less than this threshold, they can be considered as a set of inliers; otherwise, they are considered as a set of incorrect matches (outliers). The objective function is constructed in the way of the maximum consensus set to optimize the rotation estimation, so as to distinguish which observations are inliers and which are outliers, thereby suppressing the influence of outliers.

[0080] Regarding the efficiency of rotation estimation, embodiments of the present invention transform the rotation estimation problem with three degrees of freedom into a rotation axis estimation problem with two degrees of freedom and a rotation angle estimation problem with one degree of freedom by introducing a special geometric constraint, thereby greatly improving the efficiency of solving the rotation estimation problem. For the solution of the rotation axis, the present invention proposes a geometric constraint that only contains the rotation axis, and transforms the problem of solving the rotation axis into the problem of finding the most intersections of a circular ring on the unit sphere. To find this intersection, the present invention uses stereographic projection to map the circular ring from the three-dimensional sphere to the two-dimensional plane, so that calculations in the three-dimensional space can be avoided, thereby further improving the solution efficiency. To solve the rotation axis robustly and efficiently, the present invention introduces a spatial voting strategy. Using this strategy, the present invention can find all intersections of the circle on the two-dimensional plane, thereby determining an optimal rotation axis, or finding multiple rotation axes simultaneously when the input data contains multiple rotations. After obtaining the optimal rotation axis, the optimal rotation angle can be confirmed using the Rodriguez formula, thereby obtaining the optimal rotation estimation. Similarly, after confirming multiple rotation axes, multiple corresponding rotation angles are also obtained using the Rodriguez formula, thereby obtaining multiple rotation estimations.

[0081] Embodiments of the present invention first decouple the problem of solving the rotation estimation into the problems of calculating the rotation axis and the rotation angle, then establish a geometric constraint that only contains the rotation axis, transform the problem of calculating the rotation axis into the problem of finding the most intersections of a circular ring on the three-dimensional sphere, and then simplify the operation based on stereographic projection, transform the problem of finding the most intersections of a circular ring on the three-dimensional sphere into the problem of finding the most intersections of a circular ring on the two-dimensional plane, and further use stereographic projection and spatial voting technology to solve the rotation axis and the rotation angle, achieving high-efficiency and high-robustness rotation estimation, and can be applied to the estimation of multiple rotations at the same time; in addition, embodiments of the present invention have high robustness when dealing with rotation estimation problems of large-scale data (10^5) and a large number of outliers (90%), and can be applied to real-time systems.

[0082] The implementation process of embodiments of the present invention will be further described below.

[0083] Any rotation can be represented by the rotation axis and the rotation angle θ ∈ [0, π]. Referring to Figure 2 , the present invention first decouples the rotation space into the rotation axis space and the rotation angle space [0, π]. In other words, the present invention transforms the rotation estimation problem with three degrees of freedom into a rotation axis estimation problem with two degrees of freedom and a rotation angle estimation problem with one degree of freedom to reduce the time complexity.

[0084] After the present invention decouples the rotation estimation problem, the most crucial problem now lies in how to robustly and quickly solve the optimal rotation axis. For this purpose, the present invention introduces the strategy of stereographic projection, transforms the spherical optimization problem into a planar optimization problem, and thus realizes an optimization algorithm with a complexity of , and further quickly and robustly solves the problem of rotation axis estimation.

[0085] Further as an optional implementation manner, the geometric constraint is:

[0086] ∠(r, x - y) = 90°

[0087] where r represents the rotation axis to be solved, x represents the observed point cloud, and y represents the target point cloud;

[0088] Transforming the problem of calculating the rotation axis into the problem of finding the most intersection points of a circular ring on a three-dimensional sphere, which specifically includes:

[0089] S1021. For any vector point given on the three-dimensional sphere, the rotation axis satisfying the geometric constraint is perpendicular to it, so as to determine that the rotation axis satisfying the geometric constraint is within a space circular ring;

[0090] S1022. For multiple vector points given on the three-dimensional sphere, the rotation axis satisfying the geometric constraint is simultaneously within multiple space circular rings, so as to transform the problem of calculating the rotation axis into the problem of determining the orientation of the rotation axis to be solved according to the intersection points of multiple space circular rings on the three-dimensional sphere.

[0091] Specifically, according to the geometric constraint of the rotational motion, it can be found that Rx = y → r T Rx = r T y → r T x = r T y → r T (x - y) = 0. At this time, geometrically speaking, it is ∠(r, x - y) = 90°, that is, r is perpendicular to x - y. If the lengths are all normalized, define At this time, z T r = 0. Given N groups of inputs, the present invention will obtain a set of linear equations:

[0092]

[0093] Obviously, the rotation axis r can be obtained by giving two sets of input rotation axes. If more than two sets of inputs are given, the rotation axis in the least squares sense will be obtained. However, if there are outliers or errors in the input, the solution in the least squares sense will deviate significantly from the true solution.

[0094] According to the geometric structure of this linear equation, referring to Figure 3 , given any z i , which is a vector point on the spherical surface, the rotation axis r that satisfies the constraint must be perpendicular to it, so it must be within a space circular ring. If multiple z i are given, then there will be multiple circular rings, and the intersection point of them is the orientation of the corresponding rotation axis. Therefore, the problem of finding the optimal rotation axis orientation can be transformed into the problem of finding the intersection point of multiple circular rings on the spherical surface. Although the spherical surface is a regular geometric structure, the spherical surface is still a non-convex set, and finding the optimal solution on it is still a non-convex optimization problem. Therefore, the present invention introduces the strategy of stereographic projection to transform the spherical surface into a plane and solve the problem of finding the intersection point of multiple circular rings in the plane.

[0095] Furthermore, as an optional implementation manner, based on the simplification of the stereographic projection operation, the problem of finding the maximum number of intersection points of circular rings on the three-dimensional spherical surface is transformed into the problem of finding the maximum number of intersection points of circular rings on the two-dimensional plane, which specifically includes:

[0096] S1031. Use the stereographic projection to project multiple space circular rings on the three-dimensional spherical surface onto the two-dimensional plane to obtain multiple plane circular rings on the corresponding two-dimensional plane;

[0097] S1032. Transform the problem of determining the orientation of the rotation axis to be solved according to the intersection points of multiple space circular rings on the three-dimensional spherical surface into the problem of determining the orientation of the rotation axis to be solved according to the intersection points of multiple plane circular rings on the two-dimensional plane.

[0098] Specifically, the stereographic projection is a mapping that projects the spherical surface onto the plane, and this mapping has smoothness, bijectivity, and conformality. In addition, the stereographic projection also has the property of preserving circles, that is, the circular ring in the spherical surface is still a circular ring after being projected onto the plane, as shown in Figure 4 . This property can be directly applied to the problem of solving the optimal rotation axis. Specifically, given the coordinates (a, b, c) of any point on the spherical surface, there is a point (A, B) corresponding to it in the plane, and their corresponding relationship is:

[0099]

[0100] From this formula, the inverse projection formula of the stereographic projection can be derived as:

[0101]

[0102] Generally speaking, any circular ring can be regarded as the curve of the intersection of a three-dimensional sphere and a two-dimensional plane as follows:

[0103]

[0104] It can be derived from this formula that:

[0105]

[0106] Therefore, the present invention uses stereographic projection to project the circular ring on the unit sphere onto the plane. According to the circle-preserving property of stereographic projection, the circular ring on the unit sphere in three-dimensional space is projected into the two-dimensional plane space, and what is obtained is still a series of circular rings. The intersection points of these circular rings also correspond to the positions of the optimal rotation axes projected onto the two-dimensional plane.

[0107] Further as an optional implementation manner, the rotation axis is calculated based on spatial voting, and the optimal rotation axis or multiple target rotation axes are determined according to the peak value obtained by voting. Specifically, it includes:

[0108] S1041. Discretize the solution space of the two-dimensional rotation axis projection with a grid, and statistically count the number of circular rings passing through each grid area based on spatial voting;

[0109] S1042. Determine several grid areas with the most passing circular rings as target grid areas according to the peak value obtained by voting, determine the positions of the rotation axes to be solved mapped onto the two-dimensional plane according to the target grid areas, and then use the inverse operation of stereographic projection to obtain several optimal rotation axes on the three-dimensional sphere.

[0110] Refer to Figure 5 , after the operation of stereographic projection, all circular rings are mapped onto the plane, and the task of finding the optimal rotation becomes finding a point in the space such that this point is passed by the most circular rings. This point to be found is the point mapped by the direction of the optimal rotation axis. In order to more robustly find this optimal point, the present invention uses the strategies of discretization and voting.

[0111] Refer to Figure 6 and 7, discretize the solution space of the projected two-dimensional rotation axis with a grid, and then count the number of rings that fall within each small grid. The grid with the most rings passing through it is the grid where the optimal rotation axis is located. Therefore, after discretizing and counting all the rings, the present invention can find the grid where the optimal rotation axis is located using the optimal peak method. At this time, the position of the optimal rotation axis mapped to the plane can be obtained. Finally, by using the inverse operation of stereographic projection, the optimal rotation axis can be obtained on the sphere in three-dimensional space. Similarly, when there are multiple rotations in the input data, the present invention can obtain multiple target rotation axes in the above manner.

[0112] As a further optional implementation manner, calculate the rotation angle based on spatial voting, and determine the optimal rotation angle corresponding to the optimal rotation axis or the multiple target rotation angles corresponding to the multiple target rotation axes according to the peak obtained by voting. Specifically, it includes:

[0113] S1051. Determine the trigonometric function relationship of the corresponding optimal rotation angle / target rotation angle according to the optimal rotation axis / target rotation axis and the Rodriguez rotation formula;

[0114] S1052. Substitute the observed point cloud and the target point cloud as input data into the trigonometric function relationship to obtain multiple alternative rotation angles corresponding to the optimal rotation axis / target rotation axis;

[0115] S1053. Based on spatial voting, count the number of input data corresponding to each alternative rotation angle, and determine the alternative rotation angle corresponding to the most input data as the optimal rotation angle / target rotation angle corresponding to the optimal rotation axis / target rotation axis according to the peak obtained by voting.

[0116] Specifically, after obtaining the optimal rotation axis, the rotation attitude estimation problem only remains to estimate the rotation angle. The Rodriguez rotation formula is introduced as follows:

[0117] R(θ) = I + sinθ[r] × +(1 - cosθ)[r] × 2

[0118] It can be observed that only θ is unknown in this formula. Substituting it into the formula of rotational motion, we get:

[0119]

[0120] After arrangement, a trigonometric function equation about θ can be obtained, so that the optimal rotation angle can be calculated. At this time, it should be noted that for each set of input {x i , y i}, a rotation angle θ i, at this time, the optimal rotation angle can be selected by using the histogram voting method. Similarly, when there are multiple rotations in the input data, the present invention can obtain multiple target rotation angles by the above method.

[0121] Further as an optional implementation manner, the optimal rotation estimation of the observed point cloud and the target point cloud is determined according to the optimal rotation axis and the optimal rotation angle, specifically:

[0122] S1061. Substitute the optimal rotation axis and the optimal rotation angle into the Rodriguez rotation formula to obtain the optimal rotation estimation of the observed point cloud and the target point cloud.

[0123] Specifically, after obtaining the optimal rotation axis and the optimal rotation angle, the optimal rotation estimation of the overall point cloud can be restored by using the Rodriguez rotation formula.

[0124] Further as an optional implementation manner, multiple rotation estimations of the observed point cloud and the target point cloud are determined according to multiple target rotation axes and corresponding multiple target rotation angles, specifically:

[0125] S1062. Substitute the determined target rotation axis and the corresponding target rotation angle into the Rodriguez rotation formula to obtain multiple rotation estimations of the observed point cloud and the target point cloud.

[0126] Specifically, after obtaining multiple target rotation axes and target rotation angles, substitute each group of corresponding target rotation axes and target rotation angles into the Rodriguez rotation formula to restore multiple rotation estimations of the overall point cloud.

[0127] The present invention replaces the original complex optimal rotation estimation problem with three degrees of freedom with a problem with two degrees of freedom and one degree of freedom, and further solves the rotation axis and the rotation angle by using the stereographic projection and the spatial voting strategy. This method not only improves the efficiency of rotation estimation but also enhances the robustness of rotation estimation. Moreover, when there are multiple rotations in the input data, it can also solve multiple rotation estimation problems both robustly and efficiently at the same time.

[0128] The method steps of the embodiments of the present invention are described above. It can be recognized that the embodiments of the present invention first decouple the problem of solving the rotation estimation into the problems of calculating the rotation axis and the rotation angle, then establish a geometric constraint containing only the rotation axis, transform the problem of calculating the rotation axis into the problem of finding the most intersections of a circular ring on a three-dimensional sphere, and then simplify the operation based on the stereographic projection, transform the problem of finding the most intersections of a circular ring on a three-dimensional sphere into the problem of finding the most intersections of a circular ring on a two-dimensional plane, and further use the stereographic projection and the spatial voting technology to solve the rotation axis and the rotation angle, realizing high-efficiency and high-robustness rotation estimation, and can be applied to the estimation of multiple rotations at the same time; in addition, the embodiments of the present invention can process large-scale data (10 5) and is highly robust to the rotation estimation problem with a large number of outliers (90%) and can be applied to real-time systems.

[0129] Referring to Figure 8 , an embodiment of the present invention provides a rotation estimation system based on stereographic projection, including:

[0130] A rotation space decoupling module for decoupling the target rotation space to obtain a rotation axis space and a rotation angle space;

[0131] A first conversion module for establishing a geometric constraint containing only the rotation axis and converting the problem of calculating the rotation axis into the problem of finding the most intersections of a circular ring on a three-dimensional sphere;

[0132] A second conversion module for simplifying the operation based on stereographic projection and converting the problem of finding the most intersections of a circular ring on a three-dimensional sphere into the problem of finding the most intersections of a circular ring on a two-dimensional plane;

[0133] A rotation axis solving module for calculating the rotation axis based on spatial voting and determining the optimal rotation axis or multiple target rotation axes according to the peak value obtained by voting;

[0134] A rotation angle solving module for calculating the rotation angle based on spatial voting and determining the optimal rotation angle corresponding to the optimal rotation axis or multiple target rotation angles corresponding to multiple target rotation axes according to the peak value obtained by voting;

[0135] A rotation estimation module for determining the optimal rotation estimation of the observed point cloud and the target point cloud according to the optimal rotation axis and the optimal rotation angle, or determining multiple rotation estimations of the observed point cloud and the target point cloud according to multiple target rotation axes and the corresponding multiple target rotation angles.

[0136] The content in the above method embodiments is applicable to the system embodiments of the present invention. The functions specifically implemented by the system embodiments of the present invention are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those of the above method embodiments.

[0137] Referring to Figure 9 , an embodiment of the present invention provides a rotation estimation device based on stereographic projection, including:

[0138] At least one processor;

[0139] At least one memory for storing at least one program;

[0140] When the above at least one program is executed by the above at least one processor, the above at least one processor implements the above rotation estimation method based on stereographic projection.

[0141] The content in the above method embodiments is applicable to the device embodiments of the present invention. The functions specifically implemented in the device embodiments are the same as those in the above method embodiments, and the beneficial effects achieved are also the same as those in the above method embodiments.

[0142] An embodiment of the present invention further provides a computer-readable storage medium, which stores a program executable by a processor. The program executable by the processor is used to execute the above method for estimating rotation based on stereographic projection when executed by the processor.

[0143] A computer-readable storage medium according to an embodiment of the present invention can execute a method for estimating rotation based on stereographic projection provided by an embodiment of the method of the present invention, can execute any combination of implementation steps of the method embodiment, and has the corresponding functions and beneficial effects of the method.

[0144] An embodiment of the present invention also discloses a computer program product or a computer program. The computer program product or the computer program includes computer instructions, and the computer instructions are stored in a computer-readable storage medium. The processor of the computer device can read the computer instructions from the computer-readable storage medium, and the processor executes the computer instructions, so that the computer device executes Figure 1 the method shown.

[0145] In some alternative embodiments, the functions / operations mentioned in the block diagrams may not occur in the order mentioned in the operation diagrams. For example, depending on the functions / operations involved, two consecutive blocks shown may actually be executed substantially simultaneously or the above blocks can sometimes be executed in the reverse order. In addition, the embodiments presented and described in the flowcharts of the present invention are provided by way of example for the purpose of providing a more comprehensive understanding of the technology. The disclosed methods are not limited to the operations and logical flows presented herein. Alternative embodiments are foreseeable, in which the order of various operations is changed and the sub-operations described as part of a larger operation are executed independently.

[0146] In addition, although the present invention has been described in the context of functional modules, it should be understood that, unless otherwise stated to the contrary, one or more of the above-described functions and / or features may be integrated in a single physical device and / or software module, or one or more functions and / or features may be implemented in separate physical devices or software modules. It should also be understood that a detailed discussion of the actual implementation of each module is not necessary for understanding the present invention. Rather, given the attributes, functions, and internal relationships of the various functional modules in the devices disclosed herein, the actual implementation of the modules will be understood within the ordinary skills of an engineer. Thus, those skilled in the art can implement the present invention as set forth in the claims without undue experimentation. It should also be understood that the specific concepts disclosed are merely illustrative and are not intended to limit the scope of the present invention, which is determined by the full scope of the appended claims and their equivalents.

[0147] If the above functions are implemented in the form of software function units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or a part of this technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the above methods in various embodiments of the present invention. The aforementioned storage medium includes: various media such as USB flash drives, mobile hard disks, read-only memories (ROM, Read-Only Memory), random access memories (RAM, Random Access Memory), magnetic disks, or optical discs that can store program codes.

[0148] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a definable sequence list of executable instructions for implementing logical functions, and can be specifically implemented in any computer-readable medium for use by an instruction execution system, apparatus, or device (such as a computer-based system, a system including a processor, or other systems that can fetch instructions from the instruction execution system, apparatus, or device and execute the instructions), or in conjunction with these instruction execution systems, apparatuses, or devices. For the purposes of this specification, a "computer-readable medium" can be any device that can contain, store, communicate, propagate, or transport a program for use by or in conjunction with an instruction execution system, apparatus, or device.

[0149] More specific examples (a non-exhaustive list) of computer-readable media include the following: an electrical connection (electronic device) having one or more wirings, a portable computer diskette (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber device, and a portable compact disc read-only memory (CDROM). Additionally, the computer-readable media can even be paper or other suitable media on which the above programs can be printed, because the above programs can be obtained electronically, for example, by optically scanning the paper or other media, followed by editing, interpretation, or other suitable processing as necessary, and then storing them in a computer memory.

[0150] It should be understood that various parts of the present invention can be implemented by hardware, software, firmware, or a combination thereof. In the above-described embodiments, multiple steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented by hardware, as in another embodiment, any one or a combination of the following techniques well-known in the art can be used: discrete logic circuits having logic gate circuits for implementing logical functions on data signals, application-specific integrated circuits having appropriate combinational logic gate circuits, programmable gate arrays (PGAs), field-programmable gate arrays (FPGAs), etc.

[0151] In the above description of this specification, the descriptions referring to the terms "one embodiment / example", "another embodiment / example", or "certain embodiments / examples", etc. mean that the specific features, structures, materials, or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described can be combined in any one or more embodiments or examples in a suitable manner.

[0152] Although the embodiments of the present invention have been shown and described, those of ordinary skill in the art can understand that various changes, modifications, substitutions, and variations can be made to these embodiments without departing from the principles and purposes of the present invention, and the scope of the present invention is defined by the claims and their equivalents.

[0153] The above has specifically described the preferred embodiments of the present invention, but the present invention is not limited to the above embodiments. Those skilled in the art can also make various equivalent deformations or substitutions without departing from the spirit of the present invention, and these equivalent deformations or substitutions are all included within the scope defined by the claims of this application.

Claims

1. A rotation estimation method based on stereographic projection, characterized in that: The following steps are involved: Decouple the target rotation space to obtain the rotation axis space and the rotation angle space; Establish geometric constraints containing only the rotation axis, and transform the problem of calculating the rotation axis into the problem of finding the maximum number of intersection points of circular rings on a three-dimensional sphere; Based on the simplified operation of stereographic projection, the problem of finding the maximum number of intersections of circles on a three-dimensional sphere is transformed into the problem of finding the maximum number of intersections of circles on a two-dimensional plane. The rotation axis is calculated based on spatial voting, and the optimal rotation axis or multiple target rotation axes are determined according to the peak value obtained by voting; Calculating the rotation angle based on spatial voting, and determining the optimal rotation angle corresponding to the optimal rotation axis or multiple target rotation angles corresponding to multiple target rotation axes according to a peak value obtained by voting; An optimal rotation estimate of the observation point cloud and the target point cloud is determined according to the optimal rotation axis and the optimal rotation angle, or multiple rotation estimates of the observation point cloud and the target point cloud are determined according to multiple target rotation axes and corresponding multiple target rotation angles.

2. A rotation estimation method based on stereographic projection according to claim 1, characterized in that: The geometric constraints are: ∠(r,xy)=90° Among them, r represents the rotation axis to be solved, x represents the observation point cloud, and y represents the target point cloud; The problem of calculating the rotation axis is transformed into the problem of finding the maximum number of intersection points of rings on a three-dimensional sphere, which specifically includes: For any given vector point on the three-dimensional spherical surface, the rotation axis satisfying the geometric constraint is perpendicular to it, so as to determine that the rotation axis satisfying the geometric constraint is within a spatial ring; For multiple given vector points on a three-dimensional sphere, the rotation axis that satisfies the geometric constraints is simultaneously within multiple spatial rings, thereby converting the problem of calculating the rotation axis into the problem of determining the orientation of the rotation axis to be solved based on the intersection points of multiple spatial rings on the three-dimensional sphere.

3. The rotation estimation method based on stereographic projection according to claim 1, characterized in that: The simplified operation based on stereographic projection transforms the problem of finding the maximum number of intersections of rings on a three-dimensional spherical surface into the problem of finding the maximum number of intersections of rings on a two-dimensional plane, which specifically includes: Using stereographic projection, multiple spatial rings on the three-dimensional spherical surface are projected onto a two-dimensional plane to obtain multiple plane rings on the corresponding two-dimensional plane; The problem of determining the orientation of the rotation axis to be solved based on the intersection points of multiple spatial circular rings on a three-dimensional sphere is converted into the problem of determining the orientation of the rotation axis to be solved based on the intersection points of multiple planar circular rings on a two-dimensional plane.

4. The rotation estimation method based on stereographic projection according to claim 1, characterized in that: The step of calculating the rotation axis based on spatial voting and determining the optimal rotation axis or multiple target rotation axes according to the peak value obtained by voting specifically includes: The solution space of the two-dimensional rotation axis projection is discretized using a grid, and the number of rings passing through each grid area is counted based on spatial voting; According to the peak values ​​obtained by voting, several grid areas that pass through the ring the most are determined as target grid areas, and the positions of the rotation axes to be solved mapped to the two-dimensional plane are determined according to the target grid areas, and then the inverse operation of stereographic projection is used to obtain several optimal rotation axes on the three-dimensional spherical surface.

5. The rotation estimation method based on stereographic projection according to claim 1, characterized in that: The method of calculating the rotation angle based on spatial voting and determining the optimal rotation angle corresponding to the optimal rotation axis or multiple target rotation angles corresponding to multiple target rotation axes according to the peak value obtained by voting specifically includes: Determine a trigonometric function relationship between the optimal rotation angle and the target rotation angle according to the optimal rotation axis and the target rotation axis and the Rodriguez rotation formula; Substituting the observed point cloud and the target point cloud as input data into the trigonometric function relationship to obtain a plurality of candidate rotation angles corresponding to the optimal rotation axis / the target rotation axis; The number of input data corresponding to each candidate rotation angle is counted based on spatial voting, and the candidate rotation angle corresponding to the most input data is determined according to the peak value obtained by voting as the optimal rotation angle / target rotation angle corresponding to the optimal rotation axis / target rotation axis.

6. A rotation estimation method based on stereographic projection according to any one of claims 1 to 5, characterized in that: The optimal rotation estimation of the observation point cloud and the target point cloud is determined according to the optimal rotation axis and the optimal rotation angle, which is specifically: Substituting the optimal rotation axis and the optimal rotation angle into the Rodriguez rotation formula, an optimal rotation estimate of the observation point cloud and the target point cloud is obtained.

7. A rotation estimation method based on stereographic projection according to any one of claims 1 to 5, characterized in that: The method of determining multiple rotation estimates of the observation point cloud and the target point cloud according to the multiple target rotation axes and the corresponding multiple target rotation angles is specifically as follows: The target rotation axis and the corresponding target rotation angle are determined and substituted into the Rodriguez rotation formula to obtain multiple rotation estimates of the observation point cloud and the target point cloud.

8. A rotation estimation system based on stereographic projection, characterized in that: include: A rotation space decoupling module is used to decouple the target rotation space to obtain a rotation axis space and a rotation angle space; The first conversion module is used to establish geometric constraints containing only the rotation axis, and convert the problem of calculating the rotation axis into the problem of finding the maximum number of intersections of rings on a three-dimensional spherical surface; The second conversion module is used to convert the problem of finding the maximum number of intersections of rings on a three-dimensional spherical surface into the problem of finding the maximum number of intersections of rings on a two-dimensional plane based on the simplified calculation of stereographic projection; A rotation axis solving module is used to calculate the rotation axis based on spatial voting, and determine the optimal rotation axis or multiple target rotation axes according to the peak value obtained by voting; A rotation angle solving module, used for calculating the rotation angle based on spatial voting, and determining the optimal rotation angle corresponding to the optimal rotation axis or multiple target rotation angles corresponding to multiple target rotation axes according to a peak value obtained by voting; The rotation estimation module is used to determine the optimal rotation estimation of the observation point cloud and the target point cloud according to the optimal rotation axis and the optimal rotation angle, or to determine multiple rotation estimations of the observation point cloud and the target point cloud according to multiple target rotation axes and corresponding multiple target rotation angles.

9. A rotation estimation device based on stereographic projection, characterized in that: include: at least one processor; at least one memory for storing at least one program; When the at least one program is executed by the at least one processor, the at least one processor implements the rotation estimation method based on stereographic projection as described in any one of claims 1 to 7.

10. A computer-readable storage medium storing a program executable by a processor, characterized in that: The program executable by the processor is used to perform a rotation estimation method based on stereographic projection as claimed in any one of claims 1 to 7 when executed by the processor.

Images