Camera pose decoupling method and system based on light cone geometry

Abstract

This invention proposes a camera attitude decoupling method and system based on optical cone geometry. The method includes: constructing an observation optical cone descriptor using camera intrinsic parameters, distortion-corrected image points, corresponding 3D object points, and camera center priors; applying only roll and pitch transformations in the world coordinate system to obtain an intermediate representation without yaw and constructing a predicted optical cone descriptor; constructing a yaw-insensitive 2D objective function to solve for the roll and pitch angles, and then recovering the yaw angle through lateral azimuth difference; finally, refining the rotational attitude using a three-degree-of-freedom joint approach. This invention decouples the three-degree-of-freedom attitude coupling optimization into a 2D master optimization plus a 1D analytical solution, reducing parameter coupling and improving solution stability and initial value robustness. It is applicable to scenarios such as industrial measurement, visual positioning, navigation, and 3D reconstruction.

Description

Technical Field

[0001] This invention relates to the field of photogrammetry, and in particular to a camera attitude decoupling method and system based on optical cone geometry. Background Technology

[0002] Existing methods for determining camera attitude typically construct reprojection errors directly on the image plane and integrate roll, pitch, yaw, and translation parameters into a single objective function for joint optimization. While these methods exhibit good compatibility with frameworks such as PnP, BA, visual odometry, and SLAM, they often suffer from reduced optimization stability when initial biases are large, observation noise is high, or point set distribution is degraded. This can lead to narrow valleys, significant degree-of-freedom coupling, and a deterioration of the Hessian matrix condition number.

[0003] In direct reprojection models, pixel deviations at a single image point are often influenced by multiple rotational degrees of freedom and depth scales simultaneously. In particular, yaw angle variations around the camera's optical axis often create a mixed effect with roll and pitch angles on the image plane, making it difficult for optimizers to distinguish the contributions of azimuth and tilt variations to the residuals. Simply changing the parameterization forms such as Euler angles, quaternions, or Lie algebras usually only improves the derivative expression and cannot eliminate the coupling between rotational degrees of freedom at the error definition level.

[0004] From an imaging geometry perspective, an image point essentially corresponds to a spatial line of sight passing through the perspective center. If we examine the angle subtended by this line of sight relative to the optical axis with the camera's optical axis as the axis of symmetry, we can obtain a cone angle description that is separate from the azimuth angle. Rotation about the optical axis will change the orientation of the line of sight in the horizontal plane, but it will not change the angle between the line of sight and the optical axis, nor will it change the radial and axial ratio relationship in the normalized radial-axial descriptor.

[0005] However, existing engineering implementations often directly write the world point as Based on this, we discuss yaw invariance. If camera center compensation or relative position vector construction is not performed first, the translation term will destroy the simple radial-axial invariance relationship, which will prevent the mathematical proof that "yaw angle can be directly removed from the main problem" from being closed, and thus the constructed decoupling objective function will lack a strict geometric basis.

[0006] On the other hand, relying solely on cone angle descriptions results in the loss of orientation information. Therefore, without further designing yaw recovery mechanisms, degradation judgments, and reprojection verification processes, multiple solutions or instability can easily occur when the point set is nearly symmetrical, the orientation distribution is uneven, or there are insufficient visible samples. What is truly needed in engineering is to decouple yaw from the main problem while retaining an implementable closed loop for restoring the complete attitude.

[0007] Therefore, how to construct an optical cone description that is insensitive to yaw angle through camera center compensation while ensuring geometric consistency, and how to rewrite the three-degree-of-freedom rotation master problem into a phased solution process of two-dimensional master optimization and one-dimensional yaw recovery, has become a technical problem that urgently needs to be solved in this field. Summary of the Invention

[0008] To address the aforementioned shortcomings of existing technologies, the present invention aims to provide a camera attitude decoupling method and system based on optical cone geometry, thereby solving the problems of high coupling between roll angle, pitch angle, and yaw angle under a unified reprojection objective function, insufficient solution stability, and non-closed-loop proof of yaw invariance in traditional camera attitude solutions.

[0009] To solve the above-mentioned technical problems, the present invention provides the following technical solution: A camera attitude decoupling method based on optical cone geometry includes the following steps: Acquire camera intrinsic parameters, image points after distortion correction, three-dimensional object points corresponding to the image points, and the camera center position or translation prior information equivalent to the camera center position; Based on the camera center position or the translation prior information and the three-dimensional object point, construct a relative position vector, back-project the image point into a unit observation line of sight, and construct the observation light cone descriptor based on the angle relationship between the unit observation line of sight and the camera optical axis or the normalized proportional relationship between the lateral component and the axial component. In the world coordinate system with the camera center as the origin, the unit observation line of sight is kept fixed, and only the coordinate system rotation transformation corresponding to the roll angle and pitch angle is applied to the world coordinate system where the relative position vector is located, to obtain an intermediate coordinate representation without yaw angle component, and a predicted light cone descriptor is constructed based on the angle relationship between the intermediate coordinate representation and the camera optical axis or the normalized proportional relationship between the lateral component and the axial component. Based on the observed light cone descriptor and the predicted light cone descriptor, a two-dimensional objective function insensitive to yaw angle is constructed, and the roll angle and pitch angle are solved. Based on the roll and pitch angles obtained from the solution, the yaw angle is recovered according to the azimuth difference between the unit line of sight and the intermediate coordinates on the lateral plane. Using the recovered roll, pitch, and yaw angles as initial values, a three-degree-of-freedom joint least squares refinement is performed, and the camera rotation attitude is output based on the refined roll, pitch, and yaw angles and the camera center position or the updated translation prior information.

[0010] Furthermore, the unit observation line of sight is represented as:

[0011] in, For the first Each unit observes the line of sight; For the camera intrinsic parameter matrix, For the first The coordinates of the image points For the first The homogeneous coordinate vectors corresponding to each image point This indicates transpose.

[0012] Furthermore, the intermediate coordinates are represented as follows:

[0013] in, For the first A relative position vector, For the first The intermediate coordinates represent... For roll angle, The pitch angle, For the corresponding roll angle Around The rotation matrix of the axis. For the corresponding pitch angle Circling Axis rotation matrix; in numerical implementation, the intermediate coordinates are equivalently represented as the intermediate vector after applying the corresponding inverse rotation matrix to the relative position vector.

[0014] Furthermore, the observed light cone descriptor and the predicted light cone descriptor are represented by a cone angle descriptor or a normalized radial-axial vector. Cone angle descriptor , representing the angle between the corresponding line of sight or vector and the camera optical axis; Normalized radial-axial vector This represents the normalized proportional relationship between the lateral and axial components of the corresponding line of sight or vector. Among them, when constructing the observation light cone descriptor, These represent the coordinates of the unit observation line of sight in the world coordinate system; when constructing the predictive light cone descriptor, The intermediate coordinates represent the coordinates in the world coordinate system.

[0015] Furthermore, the two-dimensional objective function is expressed as:

[0016] in, Regarding the roll angle and pitch angle The two-dimensional objective function representation, For the first The observation cone descriptor constructed by backprojecting a unit observation line of view from a number of image points. For the first The intermediate coordinates corresponding to each three-dimensional object point represent the constructed predictive light cone descriptor. For the first The weight of the correspondence between image points and 3D object points in the 2D master optimization stage. For robust loss function, This is a difference measurement function.

[0017] Furthermore, yaw angle recovery includes: recovery using the weighted circular mean of the difference between the observed azimuth in the lateral plane and the predicted azimuth represented by the intermediate coordinates in the lateral plane.

[0018] in, To restore the yaw angle, For the first The observation azimuth of each unit's line of sight on the horizontal plane For the first The intermediate coordinates represent the predicted orientation on the horizontal plane. For the first The weight of the correspondence between group image points and 3D object points in the yaw recovery phase; This represents mapping the azimuth difference to a complex number representation on the unit circle. This represents the argument of the weighted summation result.

[0019] Furthermore, after recovering the yaw angle, using the recovered roll, pitch, and yaw angles as initial values, and based on the reprojection error, the difference metric error between the observed and predicted light cone descriptors, or a weighted combination of both, as the objective function, a three-degree-of-freedom joint least-squares refinement of the roll, pitch, and yaw angles is performed. Here, the reprojection error is the error between a three-dimensional object point projected onto the image plane under a projection model determined by the roll, pitch, yaw angles, and camera center position, and the corresponding distortion-corrected image point. The difference metric error between the observed and predicted light cone descriptors is the error between the observed and predicted light cone descriptors, calculated using the reprojection error, the difference metric error between the observed and predicted light cone descriptors, and the difference metric error between the observed and predicted light cone descriptors. The difference between the observed light cone descriptor constructed by backprojecting a unit line of sight from image points and the predicted light cone descriptor constructed by the camera coordinate vector under the full rotational attitude.

[0020] Furthermore, the two-dimensional objective function corresponds to the positive definiteness, minimum eigenvalue, and condition number of the Hessian matrix or approximate Hessian matrix, and can be selected or switched between the Newton method, Gauss-Newton method, quasi-Newton method, and Levenberg-Marquardt method.

[0021] Furthermore, it also includes at least one of the following robust processing methods: representing the intermediate coordinates as corresponding vectors. z Components or complete camera coordinate vectorsz Samples with components not exceeding a threshold are filtered for visibility; outlier removal is performed using RANSAC, confidence weighting, or residual thresholding; prior pose, previous frame pose, or multi-frame joint constraints are introduced when the point set distribution is symmetrical or near-degenerate.

[0022] On the other hand, the present invention also provides a camera attitude decoupling system based on optical cone geometry, comprising: The data input module is used to acquire camera intrinsic parameters, image points after distortion correction, three-dimensional object points corresponding to the image points, and the camera center position or translation prior information equivalent to the camera center position. The observation light cone descriptor construction module is used to construct a relative position vector based on the camera center position and the three-dimensional object point, back-project the image point into a unit observation line of sight, and construct the observation light cone descriptor based on the angle relationship between the unit observation line of sight and the camera optical axis or the normalized ratio relationship between the lateral component and the axial component. The predictive light cone descriptor construction module is used to keep the unit observation line fixed in the world coordinate system with the camera center as the origin, apply only the coordinate system rotation transformation corresponding to the roll angle and pitch angle to the world coordinate system where the relative position vector is located, obtain the intermediate coordinate representation without yaw angle component, and construct the predictive light cone descriptor according to the angle relationship between the intermediate coordinate representation and the camera optical axis or the normalized proportional relationship between the lateral component and the axial component. The two-dimensional decoupling optimization module is used to construct a two-dimensional objective function that is insensitive to the yaw angle based on the observed light cone descriptor and the predicted light cone descriptor, and to solve for the roll angle and pitch angle. The yaw recovery module is used to recover the yaw angle based on the roll angle and pitch angle obtained by the solution, and according to the azimuth difference between the unit line of sight and the intermediate coordinates represented on the lateral plane. The result output module is used to perform three-degree-of-freedom joint least squares refinement with the recovered roll angle, pitch angle and yaw angle as initial values, and output the camera rotation attitude based on the refined roll angle, pitch angle, yaw angle and the camera center position or the updated translation prior information.

[0023] Compared with the prior art, the present invention has the following beneficial effects: 1. This invention constructs a relative position vector through camera center compensation, mathematically correcting the direct... The discussion of the breakpoints in yaw invariance under the formal condition avoids the failure of yaw invariants caused by translation terms when camera center compensation is not performed, and enhances the geometric closed-loop property of the technical solution. 2. This invention rewrites the three-degree-of-freedom joint optimization into a solution process that combines two-dimensional master optimization, yaw analytical recovery, and three-degree-of-freedom local refinement. This reduces the coupling degree of optimization variables, improves the properties of local curvature and Hessian matrix, and enhances numerical stability, initial value robustness, and final solution accuracy. 3. The optical cone descriptor used in this invention has clear geometric interpretability and is compatible with PnP initial values, IMU attitude priors, previous frame pose, and multi-frame joint constraints, which facilitates engineering implementation.

[0024] 4. This invention can quickly output rotational pose when the camera center position is known, and can also output complete pose by alternating updates when the camera center position is unknown but there is a rough prior. Therefore, it has a wider range of applicable scenarios. Attached Figure Description

[0025] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0026] Figure 1 This is a flowchart of the method according to an embodiment of the present invention; Figure 2 This invention provides a schematic diagram of the geometric relationship between the observed light cone and the predicted light cone. Figure 3 This is a schematic diagram of the world coordinate system rotation decomposition and decoupling sequence in an embodiment of the present invention; Figure 4 This is a schematic diagram of the local surface of the objective function of the two-dimensional subproblem and the positive definite region of the Hessian matrix in an embodiment of the present invention; Figure 5 This is a schematic diagram of yaw angle recovery according to an embodiment of the present invention; Figure 6 This is a schematic diagram of a three-layer attitude solving framework based on optical cone geometry according to an embodiment of the present invention; Figure 7 This is a schematic diagram of the system structure according to an embodiment of the present invention. Detailed Implementation

[0027] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0028] The following description, in conjunction with the accompanying drawings, further illustrates this embodiment.

[0029] like Figure 1 As shown, this example provides a camera attitude decoupling method based on light cone geometry, including the following steps: Step S1: Obtain camera intrinsic parameters, image points after distortion correction, three-dimensional object points corresponding to the image points, and the camera center position or translation prior information equivalent to the camera center position; Step S2: Construct a relative position vector based on the camera center position or translation prior information and the three-dimensional object point, back-project the image point into a unit observation line of sight, and construct an observation light cone descriptor based on the unit observation line of sight; Step S3: In the world coordinate system with the camera center as the origin, keep the unit observation line of sight fixed, apply only the coordinate system rotation transformation corresponding to the roll angle and pitch angle to the world coordinate system where the relative position vector is located, to obtain the intermediate coordinate representation without yaw angle component, and construct the predicted light cone description quantity based on the intermediate coordinate representation; Step S4: Construct a two-dimensional objective function that is insensitive to yaw angle based on the observed light cone descriptor and the predicted light cone descriptor, and solve for the roll angle and pitch angle; Step S5: Based on the roll angle and pitch angle obtained by the solution, recover the yaw angle according to the azimuth difference between the unit observation line of sight and the intermediate coordinates represented on the lateral plane; Step S6: Using the recovered roll angle, pitch angle, and yaw angle as initial values, perform three-degree-of-freedom joint least squares refinement, and output the camera rotation attitude based on the refined roll angle, pitch angle, yaw angle, and the camera center position or the updated translation prior information.

[0030] In this embodiment, the world coordinate system is used. W The coordinate system representing the 3D object point is defined, and a local world coordinate system is established with the camera center as the origin for attitude calculation. The unit line of sight remains fixed in the camera coordinate system, and the camera attitude is characterized by the rotation of the world coordinate system relative to the fixed observation system. The roll angle is denoted as... φ The pitch angle is denoted as θ The yaw angle is denoted as ψ ,in φ , θ and ψ These correspond to the world coordinate system with the camera center as the origin. X axis, Y shaft and Z Rotate the coordinate system of the axes. Preferably, use the decomposition form under the column vector convention. ,in For roll angle Pitch angle and yaw angle The complete rotation matrix determined by common factors For the corresponding yaw angle Circling Axis rotation matrix, For the corresponding roll angle Circling Axis rotation matrix, For the corresponding pitch angle Circling Axis rotation matrix. Since the matrix on the right acts first, in the intermediate process of "retaining only roll and pitch angles," it is first... Function, and then by Function. To avoid the translation term destroying the yaw invariant, it is preferable to use the camera center position in the world coordinate system. C Represent translation information and construct relative position vectors. ,in, Represents the world coordinate system. The coordinates of a three-dimensional object point This represents the coordinates of the camera center in the world coordinate system. In numerical implementation, the rotation of this coordinate system can be equivalently written as a coordinate transformation by applying an inverse rotation matrix to the relative position quantities.

[0031] Figure 2 The geometric relationship between the observed and predicted light cones is given. In the observation domain, the unit line of sight obtained through back projection forms a cone angle with the optical axis; in the prediction domain, the intermediate coordinate representation obtained after camera center compensation and rotation forms the same angular relationship between the complete camera coordinate vector and the optical axis. As long as both have the same angle with the optical axis, or their normalized radial-axial descriptors are the same, they can be considered as equivalent descriptors corresponding to the same light cone.

[0032] In this embodiment, the "optical cone descriptor" is an abstract representation of the geometric relationship between the spatial line of sight corresponding to an image point and the camera's optical axis. It emphasizes directional geometric features rather than necessarily understanding a single image point as an independent, solid cone. For any distortion-corrected image point, after back-projecting it to obtain the unit observation line of sight, the line of sight can be equivalently described using the angle between it and the optical axis, or using the normalized ratio between its lateral and axial components. As long as two lines of sight have the same angle relative to the optical axis, or the same normalized radial-axial ratio, they can be considered to correspond to the same equivalent optical cone description.

[0033] Among them, the cone angle descriptor is mainly used to characterize the angle at which the line of sight is open relative to the optical axis; the normalized radial-axial vector is mainly used to characterize the relative proportional relationship between the lateral and axial components of the line of sight. Although the two have different forms of expression, they can both reflect the core geometric information related to the decoupling solution of this invention, and therefore can both be used as the implementation forms of the observed light cone descriptor and the predicted light cone descriptor.

[0034] In addition, in this embodiment, "lateral plane" refers to a plane perpendicular to the camera's optical axis; "observation azimuth" refers to the azimuth angle after a unit observation line of sight is projected onto the lateral plane; and "predicted azimuth" refers to the azimuth angle after the intermediate coordinates are projected onto the lateral plane. Since the yaw angle essentially corresponds to the azimuth change around the optical axis, after the roll and pitch angles are determined, the yaw angle can be recovered using the difference between the observation azimuth and the predicted azimuth.

[0035] Image points after distortion correction First, back projection is performed using the camera's intrinsic parameters to obtain the unit observation line of sight. : (1) in, Figure 2 In Represents the image points The unit observation line obtained by back projection, for the first The unit observation line corresponding to each image point is denoted as . ,Right now .

[0036] (2) (3) in, and These represent the components of the unit observation line of sight along the two coordinate axes of the horizontal plane, respectively. This represents the axial component of the unit observation line of sight along the camera's optical axis. This represents the radial component magnitude of the unit observation line of sight in the transverse plane; Indicates the first The cone angle descriptor corresponding to each observation point is used to characterize the angle between the unit observation line of sight and the camera optical axis; Indicates the first The normalized radial-axial vector corresponding to each observation point is used to characterize the normalized proportional relationship between the lateral and axial components of the unit observation line of sight. Using the normalized radial-axial vector avoids scale inconsistency issues. In this invention, the "observational light cone descriptor" does not require that a single image point be strictly considered an independent physical light cone, but rather refers to an equivalent description of the light cone constructed using the angular information of the unit observation line of sight relative to the optical axis corresponding to that image point.

[0037] For each 3D object point First, construct the relative position vector based on the camera center position. , Figure 3 Given the column vector convention, the world coordinate system with the camera center as the origin is first rotated around... The axis performs pitch coordinate rotation and then orbital rotation. The axis performs a roll coordinate rotation and finally rotates around the axis. The diagram illustrates the sequence of yaw coordinate rotation. The purpose of this sequence is to: keep the observation ray fixed, first complete the roll and pitch transformations of the world coordinate system in a subproblem without yaw angle to obtain an intermediate coordinate representation, and then restore the yaw angle as an independent one-dimensional post-processing variable.

[0038] Subsequently, while keeping the unit observation line of sight fixed, only the relative position vector was considered. The corresponding world coordinate system is rotated by the roll and pitch angles; in numerical implementation, this process is equivalent to performing a corresponding inverse rotation coordinate transformation on the relative position vector to obtain an intermediate coordinate representation. : (4) The camera coordinate vector under the full rotational attitude can be represented as: (5) In column vector representation, the numerical implementation of the coordinate system rotation described above can also be equivalently written as: ,in Therefore, it can be seen that if the center position of the camera is known... Therefore, this embodiment can be directly used in the main problem. This eliminates the disruption of yaw invariants caused by translation; the left multiplication of the matrix here is merely an equivalent computational form of coordinate system rotation, rather than an active rotation of the fixed observation ray. If only the translation prior is given, it can be first converted into the camera center position, or an equivalent camera center position can be updated in the subsequent alternating update phase.

[0039] To ensure the rigorous geometric basis of the yaw angle invariance proof, this embodiment preferably first performs camera center compensation on the 3D object point, rewriting the object point's absolute position in the world coordinate system as a relative position vector relative to the camera center. The reason for this is that if the absolute coordinate form containing translation terms is used directly, translation effects will be mixed into the object point's direction, thus disrupting the radial-axial proportional relationship required to be maintained when rotating around the optical axis. Only by subtracting the camera center position first to obtain the relative position vector can the subsequent analysis truly focus on the "direction relative to the camera center," laying the foundation for decoupling the yaw angle from the main problem.

[0040] In terms of geometric description, this embodiment adopts a method that keeps the unit observation line of sight fixed, interpreting attitude changes as a rotation of the world coordinate system with the camera center as the origin relative to the fixed observation reference system. Under this description, the world coordinate system is first rotated according to the pitch and roll coordinates to obtain an intermediate coordinate representation without yaw components. In the program implementation, the above-mentioned "coordinate system rotation" process can be equivalently represented as "keeping the coordinate system stationary and multiplying the relative position vector by the corresponding inverse rotation matrix on the left." The two notations are consistent in geometric meaning and numerical results. The former is convenient for explaining the decoupling principle, while the latter is convenient for engineering implementation. Therefore, the coordinate system rotation description in the specification and the inverse rotation matrix implementation in the program are not contradictory, but rather equivalent expressions of the same process at the theoretical explanation and numerical implementation levels.

[0041] Furthermore, under the aforementioned intermediate coordinate representation, the rotation around the optical axis corresponding to the yaw angle only changes the azimuth angle of the vector in the transverse plane, without changing its axial component or the magnitude of its transverse component. Therefore, regardless of whether the cone angle descriptor or the normalized radial-axial vector is used as the predictive optical cone descriptor, this descriptor is only related to the roll and pitch angles, and not to the yaw angle. It is precisely for this reason that the present invention can first construct a two-dimensional objective function containing only the roll and pitch angles, and then recover the yaw angle in a subsequent stage.

[0042] Once the roll and pitch angles are determined, the yaw angle can be recovered because the azimuth difference between the observation line of sight and the intermediate coordinates, represented on the transverse plane, mainly reflects the difference in rotation around the optical axis. Considering that the azimuth angle is a periodic variable, and 0° and 360° geometrically represent adjacent directions, they cannot be directly averaged using ordinary real numbers. This invention recovers the yaw angle by mapping the azimuth difference to a unit circle, performing a weighted average, and then taking the argument of the result. This process can more stably fuse multi-point azimuth information and avoid averaging distortion when the angle crosses the periodic boundary. The recovered yaw angle, along with the roll and pitch angles, serves as the initial value for subsequent three-degree-of-freedom joint least-squares refinement, further reducing reprojection errors and obtaining the final attitude result.

[0043] Will Written as ,in, and These represent the components of the intermediate vector along the two coordinate axes of the horizontal plane, respectively. This represents the axial component of the intermediate vector along the camera's optical axis; after applying a yaw coordinate rotation to the world coordinate system, its equivalent coordinate expression is: ,in, These represent the complete camera coordinate vectors. The components along the two coordinate axes in the horizontal plane, Represents the complete camera coordinate vector The axial component along the camera's optical axis, and , Therefore, if the predictive descriptor uses a cone angle or a normalized radial-axial vector, then the descriptor is determined solely by... The decision is determined by the yaw angle, and is independent of it. Accordingly, the predictive descriptor can be written as: (6) (7) in, and These represent the components of the intermediate vector along the two coordinate axes of the horizontal plane, respectively. This represents the axial component of the intermediate vector along the camera's optical axis. This represents the radial component magnitude of the intermediate vector in the transverse plane; Indicates the first The cone angle descriptor corresponding to each prediction point is used to characterize the angle between the intermediate vector and the camera optical axis; Indicates the first The normalized radial-axial vector corresponding to each prediction point is used to characterize the normalized proportional relationship between the lateral and axial components of the intermediate vector.

[0044] Regarding visibility handling, it is preferable to retain [the relevant information]. Or ultimately Greater than the threshold ε For the sample, during the yaw recovery phase, to avoid singularities in the lateral azimuth angle when approaching the optical axis, it is preferable to further use only samples that satisfy... and Samples with a lateral component magnitude greater than a threshold are included in the azimuth difference statistics. For samples that do not meet the criteria, filtering or weight reduction can be performed.

[0045] In one preferred embodiment, the cone angle difference is used as the residual term; in another preferred embodiment, the normalized radial-axial vector difference is used as the residual term. Regardless of the descriptive quantity used, the main objective function is only related to the roll angle. φ and pitch angle θ related: (8) (9) in, It can be determined by factors such as corner confidence, matching quality, visibility score, historical tracking stability, or depth distribution; Robust kernels such as Huber, Cauchy, or Tukey can be used. Since the main problem only contains... φ and θWith two variables, the local curvature of the objective function is more regular than that of three-degree-of-freedom joint optimization, making it easier to optimize using Newton-type, quasi-Newton-type, or LM-type methods.

[0046] like Figure 4 As shown, when performing local quadratic modeling of a two-dimensional subproblem, the local surface of the objective function and the positive definite region of the Hessian matrix can be used to determine the curvature state near the current iteration point. In practical implementation, it is preferable to calculate the gradient, Jacobian, and the true or approximate Hessian matrix H in each iteration; when H is positive definite and the minimum eigenvalue is greater than the threshold, Newton's method or Gauss-Newton's method can be used; when H is not positive definite or the condition number deteriorates significantly, the method can be automatically switched to the damped Levenberg-Marquardt method. The termination condition can be determined by combining the descent of the objective function, the parameter update, the state of the Hessian matrix, and the reprojection error to avoid premature stopping or oscillations caused by relying on a single indicator.

[0047] Once the roll and pitch angles are determined, the unit can observe the line of sight. The observation azimuth on the horizontal plane, and the intermediate coordinate representation. Predicted azimuth recovery yaw angle on the lateral plane Figure 5 This demonstrates the geometric meaning of yaw recovery, where the angle between the observed bearing and the predicted bearing is the local estimate of the yaw angle to be recovered. The observed bearing and the predicted bearing are defined as follows: and Then, the yaw angle is recovered using the weighted circular mean: (10) in, To recover the yaw angle, formula (10) is equivalent to performing a weighted circular mean estimation of the azimuth difference for all points. For scenarios with uneven azimuth distribution, the presence of outliers, or higher reprojection accuracy requirements, a one-dimensional search can be performed after the initial value is given in formula (10), or the initial value can be used directly to enter the three-degree-of-freedom joint least squares refinement to further minimize the reprojection error.

[0048] like Figure 6As shown, the attitude solution process in this embodiment can be further summarized as a three-layer solution framework. The first layer utilizes the cone angle consistency between the observed and predicted optical cone descriptors to construct a two-dimensional master optimization problem focusing only on the roll and pitch angles, thereby obtaining relatively stable roll and pitch angle estimation results. The second layer, based on the results of the first layer, uses the azimuth difference between the unit observation line of sight and the intermediate vector in the lateral plane to analytically recover the yaw angle. The third layer uses the aforementioned roll, pitch, and yaw angles as initial values ​​to perform three-degree-of-freedom joint least squares refinement to further reduce reprojection errors and output the final attitude parameters. This three-layer structure intuitively embodies the core technical route of this invention: "two-dimensional decoupled master optimization—one-dimensional yaw recovery—three-degree-of-freedom joint refinement." It retains the decoupling advantage of the optical cone geometric descriptor being insensitive to the yaw angle, and can recover the complete attitude and improve the final solution accuracy in the post-processing stage.

[0049] exist Figure 6 In the three-layer solution framework shown, after yaw recovery, a complete rotation matrix can be reassembled. The roll, pitch, and yaw angles obtained from the 2D principal optimization and azimuth error recovery are used as the initial values ​​for joint least-squares refinement. After refinement, these values ​​are combined with the camera center position C to generate the final pose result. If the camera center position needs further correction, C or the equivalent translation can be updated by minimizing the full reprojection error in one round, while keeping the refined rotational pose fixed, to output the complete pose result.

[0050] In some monocular scenarios, the camera center position cannot be directly obtained from external modules. To address this, a coarse initial value can be obtained through EPnP, RANSAC-PnP, previous frame pose propagation, stereo / depth modules, or inertial measurement units to assist in localization. This coarse initial value can then be converted into the camera center position. Then, in the k-th iteration, first fix... Performing the rotational decoupling solution of this invention yields... , and Then, fix the rotational attitude and update the position by minimizing the reprojection error or by pose filtering. When the decrease in reprojection error and the amount of parameter update are both less than a preset threshold, the alternating update stops. This alternating update method does not change the core of the invention, namely, the rotation master problem is decoupled through optical cone geometric description. Preferably, in each alternating update cycle, the initial values ​​of roll angle, pitch angle, and yaw angle are first obtained by two-dimensional master optimization and yaw recovery, then a joint least squares refinement is performed on the three, and finally the camera center position is updated by fixing the refined rotation attitude. This organically combines decoupled solution, analytical recovery, and local joint optimization, further improving the final pose accuracy while maintaining the low-dimensional stability of the master problem.

[0051] When 3D points are concentrated in a small local area, approximately symmetrical in orientation, or a large number of points are close to the same angle, the 2D master problem may exhibit multiple solutions or near-degeneracy. To address this, the following measures can be taken: First, use spatial distribution indices of the point set or the minimum eigenvalue of the Hessian matrix to detect degradation; second, introduce the attitude of the previous frame, the attitude of the inertial measurement unit, or the main orientation constraints of the scene as priors; third, rank candidate solutions using the total reprojection error after yaw recovery; and fourth, employ multi-frame joint estimation when single-frame information is insufficient.

[0052] Example 1 In a set of simulation scenarios, the verification environment was constructed using the Matlab-based simulation system "Light Cone PhotogrammetricSystem". First, the camera intrinsic parameters, camera center position, 3D object point spatial distribution, and ground truth pose parameters were set in the simulation system, and corresponding distortion-free image points, unit observation lines of sight, and observation light cone descriptors were generated. It should be noted that during this simulation, the observation light ray remained fixed, and pose changes were expressed through rotation and translation within the world coordinate system with the camera center as the origin, thus maintaining consistency with the photogrammetric semantics of this invention.

[0053] Subsequently, the simulation output data was input into both the traditional three-degree-of-freedom joint reprojection optimization method and the method of this invention. This invention first solves for the roll and pitch angles through two-dimensional principal optimization, then recovers the yaw angle from the azimuth difference, and performs joint least-squares refinement using the three initial angle values. Finally, the attitude angle error, reprojection error, convergence count, and stability index are statistically analyzed. Simulation results show that when the initial value deviation increases or the azimuth distribution of the point set is uneven, the traditional method is prone to unstable step size, slow convergence, and even oscillations, while the objective function descent process of this invention is smoother and less sensitive to the deterioration of the Hessian matrix condition number.

[0054] Example 2 In industrial measurement scenarios, when the correspondence between calibration plate corner points, workpiece feature points, or CAD model feature points is known, this correspondence can be directly used as the input for the 3D object points and image points of this invention. This invention does not change the original feature extraction, feature matching, and distortion correction processes; it only introduces optical cone geometric descriptions in the attitude solution stage, thus allowing for easy embedding into existing industrial vision systems. For scenarios with good rotation priors but where yaw estimation is easily affected by local textures, this invention can prioritize stabilizing the roll and pitch angles, and then recover the yaw angle through azimuth difference, thereby obtaining more stable attitude results.

[0055] Example 3 In visual navigation, robot localization, and 3D reconstruction scenarios, this invention can also be used as a PnP front-end, a visual odometry rotation estimation submodule, or a SLAM front-end pose solving submodule. For continuous inter-frame scenes, the pose of the previous frame can be used as the prior of the current frame, and the multi-frame co-viewpoint constraint can be combined with the two-dimensional decoupling objective function of this invention to improve stability and noise resistance in continuous scenes. If the system also provides a coarse camera center estimation, a local translation update can be performed after rotation decoupling to output a more complete pose result.

[0056] Regarding parameter initialization, if the external system has already provided a coarse attitude and camera center, this result can be directly used as the initial input for this invention. If a complete attitude is not provided, the initialization result can be obtained first using conventional PnP, inertial measurement unit attitude propagation, previous frame pose extrapolation, or visual odometry coarse solution, and then converted to the camera center position and rotation angle representation required by this invention. Since the main optimization only targets... φ and θ In practice, the impact of initialization error on the two-dimensional master problem is usually less than that on the three-degree-of-freedom joint reprojection optimization.

[0057] For reprojection verification, it is preferable to reassemble the complete rotation matrix after yaw recovery and project the 3D object points back onto the image plane to calculate the reprojection error. If the reprojection error is lower than a preset threshold, the current attitude solution is accepted; if the reprojection error is higher than the threshold, further adjustments can be made. The process involves performing a one-dimensional search, making local corrections to the camera center, or simultaneously reducing the weights of outliers and re-entering the two-dimensional main optimization phase, thus forming a closed-loop solution process.

[0058] From a computational complexity perspective, this invention rewrites the original joint principal optimization, which might involve three degrees of freedom or even translation, into a two-dimensional principal optimization with only two variables. Although the yaw angle still needs to be recovered and reprojected for verification in subsequent stages, the reduction in the principal optimization dimension can effectively reduce the search space, improve the properties of the Hessian matrix, and reduce the solver's sensitivity to initial values. Therefore, it has significant advantages in engineering systems with high real-time requirements.

[0059] Example 4 Based on the above method, this embodiment also provides a camera attitude decoupling system based on optical cone geometry, such as... Figure 7 As shown, it includes: The data input module is used to acquire camera intrinsic parameters, image points after distortion correction, three-dimensional object points corresponding to the image points, and the camera center position or translation prior information equivalent to the camera center position. The observation light cone descriptor construction module is used to construct a relative position vector based on the camera center position and the three-dimensional object point, back-project the image point into a unit observation line of sight, and construct the observation light cone descriptor based on the angle relationship between the unit observation line of sight and the camera optical axis or the normalized ratio relationship between the lateral component and the axial component. The predictive light cone descriptor construction module is used to keep the unit observation line fixed in the world coordinate system with the camera center as the origin, apply only the coordinate system rotation transformation corresponding to the roll angle and pitch angle to the world coordinate system where the relative position vector is located, obtain the intermediate coordinate representation without yaw angle component, and construct the predictive light cone descriptor according to the angle relationship between the intermediate coordinate representation and the camera optical axis or the normalized proportional relationship between the lateral component and the axial component. The two-dimensional decoupling optimization module is used to construct a two-dimensional objective function that is insensitive to the yaw angle based on the observed light cone descriptor and the predicted light cone descriptor, and to solve for the roll angle and pitch angle. The yaw recovery module is used to recover the yaw angle based on the roll angle and pitch angle obtained by the solution, and according to the azimuth difference between the unit line of sight and the intermediate coordinates represented on the lateral plane. The result output module is used to perform three-degree-of-freedom joint least squares refinement with the recovered roll angle, pitch angle and yaw angle as initial values, and output the camera rotation attitude based on the refined roll angle, pitch angle, yaw angle and the camera center position or the updated translation prior information.

[0060] Based on the above embodiments, this embodiment also includes a robustness and verification module, which is responsible for performing visibility filtering, outlier removal, degradation judgment, Hessian matrix analysis, and reprojection verification; the result output module is responsible for outputting the rotational attitude and outputting the complete pose when needed.

[0061] It should be understood that any parts not described in detail in this specification belong to the prior art.

[0062] It should be understood that the above description of the preferred embodiments is quite detailed, but this should not be construed as limiting the scope of protection of this invention. It is neither necessary nor possible to exhaustively describe all possible implementations. Those skilled in the art, guided by this invention, can make substitutions or modifications without departing from the scope of the claims, all of which fall within the scope of protection of this invention. The scope of protection of this invention should be determined by the appended claims.

Claims

1. A camera pose decoupling method based on light cone geometry, characterized in that, Includes the following steps: Acquire camera intrinsic parameters, image points after distortion correction, three-dimensional object points corresponding to the image points, and the camera center position or translation prior information equivalent to the camera center position; Based on the camera center position or the translation prior information and the three-dimensional object point, a relative position vector is constructed. The image point is back-projected into a unit observation line of sight, and an observation light cone descriptor is constructed based on the unit observation line of sight. In the world coordinate system with the camera center as the origin, the unit observation line of sight is kept fixed, and only the coordinate system rotation transformation corresponding to the roll angle and pitch angle is applied to the world coordinate system where the relative position vector is located to obtain an intermediate coordinate representation without yaw angle component, and the predicted light cone descriptor is constructed based on the intermediate coordinate representation. Based on the observed light cone descriptor and the predicted light cone descriptor, a two-dimensional objective function insensitive to yaw angle is constructed, and the roll angle and pitch angle are solved. Based on the roll and pitch angles obtained from the solution, the yaw angle is recovered according to the azimuth difference between the unit line of sight and the intermediate coordinates on the lateral plane. Using the recovered roll, pitch, and yaw angles as initial values, a three-degree-of-freedom joint least squares refinement is performed, and the camera rotation attitude is output based on the refined roll, pitch, and yaw angles and the camera center position or the updated translation prior information.

2. The camera attitude decoupling method based on optical cone geometry according to claim 1, characterized in that, The unit observation line of sight is represented as: in, For the first Each unit observes the line of sight; For the camera intrinsic parameter matrix, For the first The coordinates of the image points For the first The homogeneous coordinate vectors corresponding to each image point This indicates transpose.

3. The camera attitude decoupling method based on optical cone geometry according to claim 1 or 2, characterized in that, The intermediate coordinates are represented as follows: in, For the first A relative position vector, For the first The intermediate coordinates represent... For roll angle, The pitch angle, For the corresponding roll angle Around The rotation matrix of the axis. For the corresponding pitch angle Circling Axis rotation matrix; in numerical implementation, the intermediate coordinates are equivalently represented as the intermediate vector after applying the corresponding inverse rotation matrix to the relative position vector.

4. The camera attitude decoupling method based on optical cone geometry according to claim 1, characterized in that, The observed and predicted light cone descriptors are represented by cone angle descriptors or normalized radial-axial vectors. Cone angle descriptor , representing the angle between the corresponding line of sight or vector and the camera optical axis; Normalized radial-axial vector This represents the normalized proportional relationship between the lateral and axial components of the corresponding line of sight or vector. Among them, when constructing the observation light cone descriptor, These represent the coordinates of the unit observation line of sight in the world coordinate system; when constructing the predictive light cone descriptor, The intermediate coordinates represent the coordinates in the world coordinate system.

5. The camera attitude decoupling method based on optical cone geometry according to claim 1, characterized in that, The two-dimensional objective function is expressed as: in, Regarding the roll angle and pitch angle The two-dimensional objective function representation, For the first The observation cone descriptor constructed by backprojecting a unit observation line of view from a number of image points. For the first The intermediate coordinates corresponding to each three-dimensional object point represent the constructed predictive light cone descriptor. For the first The weight of the correspondence between group image points and 3D object points in the 2D principal optimization stage. For robust loss function, This is a difference measurement function.

6. The camera attitude decoupling method based on optical cone geometry according to claim 1, characterized in that, Recovery of the yaw angle includes: recovery using the weighted circular mean of the difference between the observed azimuth in the lateral plane and the predicted azimuth in the lateral plane represented by the intermediate coordinates. in, To restore the yaw angle, For the first The observation azimuth of each unit's line of sight on the horizontal plane For the first The intermediate coordinates represent the predicted orientation on the horizontal plane. For the first The weight of the correspondence between group image points and 3D object points in the yaw recovery phase; This represents mapping the azimuth difference to a complex number representation on the unit circle. This represents the argument of the weighted summation result.

7. The camera attitude decoupling method based on optical cone geometry according to claim 1, characterized in that, After recovering the yaw angle, using the recovered roll, pitch, and yaw angles as initial values, a three-degree-of-freedom joint least-squares refinement of the roll, pitch, and yaw angles is performed based on the reprojection error, the difference metric error between the observed and predicted light cone descriptors, or a weighted combination of both, as the objective function. The reprojection error is the error between a 3D object point projected onto the image plane using a projection model determined by the roll, pitch, yaw angles, and camera center position, and the corresponding distortion-corrected image point. The difference metric error between the observed and predicted light cone descriptors is the error between the observed and predicted light cone descriptors. The difference between the observed light cone descriptor constructed by the unit observation line of view obtained by backprojecting image points and the predicted light cone descriptor constructed by the camera coordinate vector under the full rotation attitude.

8. The camera attitude decoupling method based on optical cone geometry according to claim 5, characterized in that, The two-dimensional objective function corresponds to the positive definiteness, minimum eigenvalue, and condition number of the Hessian matrix or approximate Hessian matrix, and can be selected or switched between Newton's method, Gauss-Newton method, quasi-Newton method, and Levenberg-Marquardt method.

9. The camera attitude decoupling method based on optical cone geometry according to claim 1, characterized in that, It also includes at least one of the following robust processing methods: representing the intermediate coordinates as corresponding vectors z Components or complete camera coordinate vectors z Samples with components not exceeding a threshold are filtered for visibility; outlier removal is performed using RANSAC, confidence weighting, or residual thresholding; prior pose, previous frame pose, or multi-frame joint constraints are introduced when the point set distribution is symmetrical or near-degenerate.

10. A camera attitude decoupling system based on optical cone geometry, characterized in that, include: The data input module is used to acquire camera intrinsic parameters, image points after distortion correction, three-dimensional object points corresponding to the image points, and the camera center position or translation prior information equivalent to the camera center position. The observation light cone descriptor construction module is used to construct a relative position vector based on the camera center position and the three-dimensional object point, back-project the image point into a unit observation line of sight, and construct the observation light cone descriptor based on the angle relationship between the unit observation line of sight and the camera optical axis or the normalized ratio relationship between the lateral component and the axial component. The predictive light cone descriptor construction module is used to keep the unit observation line fixed in the world coordinate system with the camera center as the origin, apply only the coordinate system rotation transformation corresponding to the roll angle and pitch angle to the world coordinate system where the relative position vector is located, obtain the intermediate coordinate representation without yaw angle component, and construct the predictive light cone descriptor according to the angle relationship between the intermediate coordinate representation and the camera optical axis or the normalized proportional relationship between the lateral component and the axial component. The two-dimensional decoupling optimization module is used to construct a two-dimensional objective function that is insensitive to the yaw angle based on the observed light cone descriptor and the predicted light cone descriptor, and to solve for the roll angle and pitch angle. The yaw recovery module is used to recover the yaw angle based on the roll angle and pitch angle obtained by the solution, and according to the azimuth difference between the unit line of sight and the intermediate coordinates represented on the lateral plane. The result output module is used to perform three-degree-of-freedom joint least squares refinement with the recovered roll angle, pitch angle and yaw angle as initial values, and output the camera rotation attitude based on the refined roll angle, pitch angle, yaw angle and the camera center position or the updated translation prior information. The camera attitude decoupling system based on optical cone geometry is used to perform the steps in the camera attitude decoupling method based on optical cone geometry according to any one of claims 1-9.

Images