Visual observation construction method based on tangent space orthogonal ray projection

Abstract

This invention relates to a visual observation construction method based on orthogonal ray projection in tangent space, belonging to the field of computer vision and robot state estimation technology. The invention proposes an orthogonal ray projection error model defined within the tangent space of the observation ray. Specifically, the method involves: transforming two-dimensional observation points in the image into observation rays on a unit sphere, constructing an orthogonal basis in the tangent space of these rays; projecting the three-dimensional landmark points to be estimated onto this tangent space basis, and calculating the orthogonal distance between these landmark points and the observation rays as the residual. This method geometrically eliminates the explicit dependence of residual calculation on scalar depth, making the error function linear with respect to feature point position parameters. The visual measurement device includes a feature extraction module, a tangent space construction module, a residual and derivative calculation module, and a state estimation module. This invention provides an accurate Jacobian matrix, significantly improving the numerical stability, convergence speed, and estimation accuracy of visual SLAM and 3D reconstruction systems in scenarios with distant feature points and weak parallax.

Description

Technical Field

[0001] This invention belongs to the field of computer vision and robot state estimation technology, and relates to a method for constructing a visual observation model in visual simultaneous localization and mapping, visual odometry and 3D reconstruction. Background Technology

[0002] In fields such as autonomous mobile robots, drones, and augmented reality, visual odometry and visual SLAM are core technologies for achieving high-precision localization and environmental perception. These systems achieve robust state estimation in complex environments by fusing visual feature observations with motion priors. In feature-point-based visual simultaneous localization and mapping or visual odometry systems, the visual measurement model is a key factor determining the consistency, numerical stability, and trajectory accuracy of the estimator. Existing mainstream methods typically employ the standard reprojection error model, which projects the estimated 3D feature points onto a normalized image plane and calculates their Euclidean distance to the observed pixels. However, the standard reprojection error model suffers from significant geometric limitations, such as high nonlinearity, sensitivity to depth uncertainty, and convergence difficulties.

[0003] To alleviate the aforementioned problems, existing technologies have proposed methods such as inverse depth parameterization. Although inverse depth parameterization improves the parameterization form of state variables, enabling the representation of points at infinity, it still relies on standard planar reprojection residuals at its underlying level. Therefore, the fundamental geometric defect caused by perspective division—the nonlinearity of the error function with respect to feature locations—remains unresolved. Summary of the Invention

[0004] To address the issues of high nonlinearity and sensitivity to depth parameters caused by perspective division in traditional visual reprojection error models, this invention aims to provide a visual measurement method based on tangent space orthogonal ray projection, which can eliminate the nonlinearity caused by perspective division and improve the numerical stability and estimation accuracy of the visual measurement model in long-distance and weak parallax scenarios.

[0005] To achieve the above objectives, the present invention adopts the following technical solution:

[0006] The visual measurement method based on tangent space orthogonal ray projection disclosed in this invention includes the following steps:

[0007] Step S1: Extract feature points from the image data acquired by the camera and determine the two-dimensional observation coordinates of the feature points on the image plane;

[0008] Step S2: Based on the camera intrinsic parameter model, convert the two-dimensional observation coordinates obtained in S1 into unit observation ray vectors in the camera coordinate system. ;

[0009] Step S3: Construct vectors orthogonal tangent space basis The orthogonal tangent spatial base Zhang Cheng and A vertical two-dimensional tangent plane;

[0010] Step S4: Obtain the estimated position vector of the 3D feature points that match the image plane feature points in the current camera coordinate system. ;

[0011] Step S5: Construct orthogonal ray projection residuals The residual is obtained by... Direction vector projected onto the base The above calculation is obtained; and the residual is calculated. Jacobian matrix of optimization variables The optimization variables include camera pose and world coordinates of 3D feature points.

[0012] Step S6: Determine the covariance matrix of the observation noise. Based on the residual Jacobian matrix and covariance matrix A probabilistic state estimation model is constructed and solved to update the camera pose and the world coordinates of 3D feature points, thereby completing the visual measurement; wherein, the solution method of the probabilistic state estimation model includes a filter-based recursive estimation method or a factor graph-based optimization method.

[0013] Further, in step S3, the orthogonal tangent spatial base The construction method is as follows: select a vector that corresponds to the unit observation ray vector. Non-parallel reference axis vectors; the first basis vectors are obtained using the Schmidt orthogonalization method. , making Perpendicular to ; Calculate the second basis vector Construct the orthogonal tangent space basis. ,satisfy and ,in It is a 2×2 identity matrix.

[0014] Further, in step S5, the orthogonal ray projection residual The formula is:

[0015]

[0016] in, Represents the magnitude of a vector. The unit direction vector represents the estimated position vector.

[0017] Further, in step S5, the Jacobian matrix Including the Jacobian matrix related to camera pose and the Jacobian matrix of the world coordinates of the three-dimensional feature points The calculation formulas are as follows: First, calculate the residual. Regarding the estimated position vector in the camera coordinate system Jacobian matrix :

[0018]

[0019] Then calculate and :

[0020]

[0021]

[0022] in, It is a 3×3 identity matrix. For vectors antisymmetric matrix, This represents the rotation matrix component in the camera pose.

[0023] Further, in step S6, the determination of the covariance matrix of the observation noise... The process specifically includes: obtaining the pixel noise covariance matrix of the feature points on the image plane. Based on camera focal length The pixel noise covariance is propagated to the tangent space to obtain the observation noise covariance matrix. Its approximate calculation formula is:

[0024]

[0025] And using the inverse matrix of the observed noise covariance matrix As an information matrix, the orthogonal ray projection residuals are weighted. In order to construct the probability state estimation model.

[0026] Further, in step S6, the filter-based recursive estimation method specifically includes: first, using the Jacobian matrix to construct a model containing camera state errors. and the world coordinate error of 3D feature points The linearized residual equation:

[0027]

[0028] in, To observe noise; next, the Jacobian matrix with respect to the world coordinates of the three-dimensional feature points is... Perform matrix decomposition to obtain the left null space projection matrix. , making ; using the left null space projection matrix The linearized residual equation and the observation noise covariance matrix Projection is performed to obtain the projection residual. and projection noise covariance :

[0029]

[0030] This eliminates the dependence of the observation equation on the positional error of the three-dimensional feature points; finally, the residuals after projection are utilized. Projected Jacobian matrix and projection noise covariance Calculate Kalman gain The camera state within the sliding window is updated to complete the visual measurement; the calculation formula for the state update is as follows:

[0031]

[0032]

[0033] in, Let be the covariance matrix of the camera state. The camera status before the update. This is the updated camera status.

[0034] Further, in step S6, the factor graph-based optimization method specifically includes: constructing a factor graph containing camera pose nodes and 3D feature point world coordinate nodes, and applying the orthogonal ray projection residuals calculated in step S5. Defined as the visual factor connecting each node; utilizing the Jacobian matrix with respect to the camera pose. and the Jacobian matrix of the world coordinates of the three-dimensional feature points Construct a first-order Taylor expansion approximation of the residual:

[0035]

[0036] in, It is a joint state vector containing the camera pose and the world coordinates of 3D feature points. The corresponding joint state increment vector is the Lie algebra increment of the camera pose. and the world coordinate increment of 3D feature points composition;

[0037] make And combined with the inverse matrix of the observed noise covariance matrix Construct the joint state increment vector The normal equation:

[0038]

[0039] The optimal joint state increment vector is obtained by solving the normal equation. and extract from it and The camera pose and 3D feature point coordinates are updated separately until convergence, thus completing the visual measurement.

[0040] This invention also discloses a visual measurement device based on tangent space orthogonal ray projection, used to implement a visual measurement method based on tangent space orthogonal ray projection. The visual measurement device based on tangent space orthogonal ray projection includes a feature extraction module, a tangent space construction module, a residual and derivative calculation module, and a state estimation module. The feature extraction module is used to acquire image data collected by a camera, extract feature points, and determine their two-dimensional observation coordinates on the image plane. The tangent space construction module is used to convert the two-dimensional observation coordinates into unit observation ray vectors and construct an orthogonal tangent space basis perpendicular to the ray. The residual and derivative calculation module is used to acquire the three-dimensional feature point vector to be estimated, project it onto the orthogonal tangent space basis to calculate the orthogonal ray projection residual, and calculate the Jacobian matrix of the residual with respect to optimization variables, including the camera pose and the three-dimensional feature point coordinates. The state estimation module is used to determine the covariance matrix of the observation noise, construct a probabilistic state estimation model based on the residual, the Jacobian matrix, and the covariance matrix, and solve the model to update the camera pose and the three-dimensional feature point coordinates, thereby completing the visual measurement.

[0041] Beneficial effects:

[0042] 1. The visual measurement method disclosed in this invention, based on orthogonal ray projection in tangent space, defines the orthogonal ray projection error in tangent space, eliminating perspective division. The residual function is linear with respect to feature point positions, making the Jacobian matrix more accurate and greatly expanding the convergence domain of the optimization solver.

[0043] 2. The visual measurement method based on tangent space orthogonal ray projection disclosed in this invention determines that the magnitude of the orthogonal ray projection error depends only on the angle error, thus decoupling it from the scalar depth of the feature points. This invention enables the system to handle feature points from near to infinity naturally and robustly, without requiring special parameter switching for distant points.

[0044] 3. The visual measurement method based on tangent space orthogonal ray projection disclosed in this invention ensures the orthogonality of the gradient by retaining the orthogonal projection terms in the Jacobian matrix, which significantly improves the numerical stability and consistency of the estimator in weak parallax and violent motion scenarios.

[0045] 4. The visual measurement method based on tangent space orthogonal ray projection disclosed in this invention has a computational complexity of orthogonal ray projection error that is comparable to that of traditional reprojection error. It is easy to integrate into existing filtering-based or optimization-based SLAM systems without affecting real-time performance, and is applicable to tasks such as 3D reconstruction and motion recovery of structures. Attached Figure Description

[0046] Figure 1 This is a flowchart of the visual measurement method based on tangent space orthogonal ray projection of the present invention.

[0047] Figure 2 This is a geometrical comparison diagram of the orthogonal ray projection error proposed in this invention and the traditional reprojection error. Figure 2 (a) represents the traditional reprojection error. Figure 2 (b) represents the orthogonal ray projection error.

[0048] Figure 3 This is a comparison chart of the sensitivity analysis of the method of this invention and the traditional method under different disturbance conditions. Figure 3 (a) is a diagram showing the sensitivity analysis of camera pose disturbance. Figure 3 (b) is a sensitivity analysis diagram of feature point location perturbation. Detailed Implementation

[0049] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments.

[0050] Example 1: Specific steps of the visual measurement method based on filter recursive estimation

[0051] like Figure 1 As shown in the figure, the visual measurement method based on tangent space orthogonal ray projection disclosed in this embodiment has the following specific implementation steps:

[0052] Step S1: Extract feature points from the image data acquired by the camera and determine the two-dimensional observation coordinates of the feature points on the image plane. ;

[0053] Step S2: Utilize the camera's intrinsic parameter model By back-projecting the pixel coordinates and normalizing them, the unit observation ray vector in the camera coordinate system is obtained. :

[0054]

[0055] This step unifies the observations from different cameras onto a single sphere, eliminating the influence of intrinsic parameter distortion.

[0056] Step S3: In order to define the error metric, it is necessary to observe the ray. We construct a local tangent plane at the top. We need to compute a set of orthogonal bases. The base is formed The zero space. The specific construction process is as follows:

[0057] 1. Select one that matches Non-parallel canonical axis vectors (e.g.) ).

[0058] 2. Generate the first basis vector using the Schmitt orthogonalization process. :

[0059]

[0060] in This is the generated unnormalized intermediate vector.

[0061] 3. Use the cross product to generate the second basis vector. :

[0062]

[0063] At this time, the matrix satisfy ,and ,in It is a 2×2 identity matrix. For example... Figure 2 As shown in (b) This forms a tangential base orthogonal to the observed ray;

[0064] Step S4: Optimize variables including camera pose (rotation) Translation ) and world coordinates of three-dimensional feature points First, the feature points are transformed to the current camera coordinate system to obtain the estimated position vector in the camera coordinate system. :

[0065]

[0066] Step S5: Construct orthogonal ray projection residuals. Unlike traditional methods... Projected onto the image plane, the present invention will Normalized projection onto the tangent space basis superior:

[0067]

[0068] The geometric meaning of this residual is the projection of the angular error between the estimated direction and the observed ray onto the tangent plane. Because the formula uses... The magnitude of the residual depends only on the direction, and not on the scalar depth of the feature point. This is irrelevant, thus achieving deep decoupling.

[0069] Simultaneously calculate the residual Jacobian matrix of optimization variables The optimization variables include camera pose and world coordinates of 3D feature points.

[0070] First, calculate the residual. Regarding the estimated position vector in the camera coordinate system Jacobian matrix :

[0071]

[0072] Then calculate the Jacobian matrix with respect to the camera pose. and the Jacobian matrix of the world coordinates of the three-dimensional feature points :

[0073]

[0074]

[0075] in, It is a 3×3 identity matrix. For vectors antisymmetric matrix, This represents the rotation matrix component in the camera pose.

[0076] Step S6: Determine the covariance matrix of the observation noise. ; and based on the residual Jacobian matrix and covariance matrix Visual measurement is achieved through a filter-based recursive estimation method. First, the pixel noise covariance matrix of the feature points on the image plane is obtained. Based on camera focal length The pixel noise covariance is propagated to the tangent space to obtain the observation noise covariance matrix. Its approximate calculation formula is:

[0077]

[0078] Using the Jacobian matrix and Constructing a system that includes camera state errors and the world coordinate error of 3D feature points The linearized residual equation:

[0079]

[0080] in, To observe noise; next, the Jacobian matrix with respect to the world coordinates of the three-dimensional feature points is... Perform matrix decomposition to obtain the left null space projection matrix. , making ; using the left null space projection matrix The linearized residual equation and the observation noise covariance matrix Projection is performed to obtain the projection residual. and projection noise covariance :

[0081]

[0082] This eliminates the dependence of the observation equation on the positional error of the three-dimensional feature points; finally, the residuals after projection are utilized. Projected Jacobian matrix and projection noise covariance Calculate Kalman gain The camera state within the sliding window is updated to complete the visual measurement; the calculation formula for the state update is as follows:

[0083]

[0084]

[0085] in, Let be the covariance matrix of the camera state. The camera status before the update. This is the updated camera status.

[0086] Example 2: Specific steps of the visual measurement method based on factor graph optimization

[0087] like Figure 1 As shown, the visual measurement method based on tangent space orthogonal ray projection disclosed in this embodiment has the same steps S1-S5 as those in Embodiment 1, except that:

[0088] Step S6: Determine the covariance matrix of the observation noise. ; and based on the residual Jacobian matrix and covariance matrix Visual measurement is achieved through an optimization method based on factor graphs.

[0089] First, obtain the pixel noise covariance matrix of the feature points on the image plane. Based on camera focal length The pixel noise covariance is propagated to the tangent space to obtain the observation noise covariance matrix. Its approximate calculation formula is:

[0090]

[0091] And using the inverse matrix of the observed noise covariance matrix As an information matrix, the projection residual of the orthogonal ray Weighting is applied.

[0092] Construct a factor graph containing camera pose nodes and 3D feature point world coordinate nodes, and use the orthogonal ray projection residuals calculated in step S5. Defined as the visual factor connecting each node; utilizing the Jacobian matrix with respect to the camera pose. and the Jacobian matrix of the world coordinates of the three-dimensional feature points Construct a first-order Taylor expansion approximation of the residual:

[0093]

[0094] in, It is a joint state vector containing the camera pose and the world coordinates of 3D feature points. The corresponding joint state increment vector is the Lie algebra increment of the camera pose. and the world coordinate increment of 3D feature points composition;

[0095] make And combined with the inverse matrix of the observed noise covariance matrix Construct the joint state increment vector The normal equation:

[0096]

[0097] The optimal joint state increment vector is obtained by solving the normal equation. and extract from it and The camera pose and 3D feature point coordinates are updated separately until convergence, thus completing the visual measurement.

[0098] like Figure 3As shown in (a), this figure illustrates the trend of the linearization error of the two projection error models with respect to the camera pose as a function of feature depth when the camera pose is disturbed. The figure shows that, at the same feature depth, the linearization error of the orthogonal ray projection error is consistently smaller than the reprojection error. This indicates that the orthogonal ray projection model maintains a smaller error and exhibits better stability when dealing with camera pose disturbances. Figure 3 As shown in (b), this figure illustrates the trend of linearization error of the two projection error models with respect to the relative position of feature points as a function of feature depth when there is a perturbation in the feature point position. The figure shows that the linearization error of the reprojection error decreases only slowly with increasing depth and remains at a relatively high level, while the linearization error of the orthogonal ray projection error is extremely small. This fully demonstrates that the orthogonal ray projection model is extremely insensitive to perturbations in the feature point position, exhibiting very high robustness. In summary, this invention, by constructing orthogonal projection errors in the tangent space, fundamentally changes the geometric constraint form of visual measurement, solves the nonlinearity and far-point instability problems caused by perspective division in traditional methods, and significantly improves the robustness and accuracy of the visual localization and reconstruction system.

[0099] The above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the technical solutions of the present invention, and these modifications or equivalent substitutions cannot cause the modified technical solutions to deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. A visual measurement method based on orthogonal ray projection in tangent space, characterized in that: Includes the following steps: Step S1: Extract feature points from the image data acquired by the camera, and determine the two-dimensional observation coordinates of the feature points on the image plane; Step S2: Based on the camera intrinsic parameter model, convert the two-dimensional observation coordinates obtained in S1 into unit observation ray vectors in the camera coordinate system. ; Step S3: Construct vectors orthogonal tangent space basis The orthogonal tangent spatial base Zhang Cheng and A vertical two-dimensional tangent plane; Step S4: Obtain the estimated position vector of the 3D feature points that match the image plane feature points in the current camera coordinate system. ; Step S5: Construct orthogonal ray projection residuals The residual is obtained by... Direction vector projected onto the base The above calculation is obtained; the residual is calculated. Jacobian matrix of optimization variables The optimization variables include camera pose and world coordinates of 3D feature points; Step S6: Determine the covariance matrix of the observation noise. Based on the residual Jacobian matrix and covariance matrix A probabilistic state estimation model is constructed and solved to update the camera pose and the world coordinates of 3D feature points, thereby achieving visual measurement.

2. The method according to claim 1, characterized in that: In step S3, the orthogonal tangent spatial base The construction method is as follows: select a vector that corresponds to the unit observation ray vector. Non-parallel reference axis vectors; the first basis vectors are obtained using the Schmidt orthogonalization method. , making Perpendicular to ; Calculate the second basis vector Construct the orthogonal tangent space basis. ,satisfy and ,in It is a 2×2 identity matrix.

3. The method according to claim 1, characterized in that: In step S5, the orthogonal ray projection residual The formula is: in, Represents the magnitude of a vector. The unit direction vector represents the estimated position vector.

4. The method according to claim 3, characterized in that: In step S5, the Jacobian matrix Including the Jacobian matrix related to camera pose and the Jacobian matrix of the world coordinates of the three-dimensional feature points ; The calculation formulas are as follows: First, calculate the residual. Regarding the estimated position vector in the camera coordinate system Jacobian matrix : Then calculate and : in, It is a 3×3 identity matrix. For vectors antisymmetric matrix, This represents the rotation matrix component in the camera pose.

5. The method according to claim 1, characterized in that: In step S6, the covariance matrix of the observation noise is determined. The process specifically includes: obtaining the pixel noise covariance matrix of the feature points on the image plane. Based on camera focal length The pixel noise covariance is propagated to the tangent space to obtain the observation noise covariance matrix. Its approximate calculation formula is: And using the inverse matrix of the observed noise covariance matrix As an information matrix, the projection residual of the orthogonal ray Weighting is applied to construct the probabilistic state estimation model.

6. The method according to claim 1, characterized in that: In step S6, the filter-based recursive estimation method specifically includes: first, using the Jacobian matrix to construct a model containing camera state errors. and the world coordinate error of 3D feature points The linearized residual equation: in, To observe noise; next, the Jacobian matrix with respect to the world coordinates of the three-dimensional feature points is... Perform matrix decomposition to obtain the left null space projection matrix. , making ; using the left null space projection matrix The linearized residual equation and the observation noise covariance matrix Projection is performed to obtain the projection residual. and projection noise covariance : The above equation eliminates the dependence of the observation equation on the position error of the three-dimensional feature points; the residual after projection is used. Projected Jacobian matrix and projection noise covariance Calculate Kalman gain The camera state within the sliding window is updated to complete the visual measurement; the calculation formula for the state update is as follows: in, Let be the covariance matrix of the camera state. The camera status before the update. This is the updated camera status.

7. The method according to claim 1, characterized in that: In step S6, the factor graph-based optimization method specifically includes: constructing a factor graph containing camera pose nodes and 3D feature point world coordinate nodes, and applying the orthogonal ray projection residuals calculated in step S5. Defined as the visual factor connecting each node; utilizing the Jacobian matrix with respect to the camera pose. and the Jacobian matrix of the world coordinates of the three-dimensional feature points Construct a first-order Taylor expansion approximation of the residual: in, It is a joint state vector containing the camera pose and the world coordinates of 3D feature points. The corresponding joint state increment vector is the Lie algebra increment of the camera pose. and the world coordinate increment of 3D feature points composition; make And combined with the inverse matrix of the observed noise covariance matrix Construct the joint state increment vector The normal equation: The optimal joint state increment vector is obtained by solving the normal equation. and extract from it and The camera pose and 3D feature point coordinates are updated separately until convergence, thus completing the visual measurement.

8. The method according to claim 1, characterized in that: The solution methods for the probabilistic state estimation model include a filter-based recursive estimation method and a factor graph-based optimization method.

9. A visual measurement device based on tangent space orthogonal ray projection, used to implement the method as described in claim 1, 2, 3, 4, 5, 6, 7 or 8, characterized in that: It includes a feature extraction module, a tangent space construction module, a residual and derivative calculation module, and a state estimation module; The feature extraction module is used to acquire image data collected by the camera, extract feature points, and determine their two-dimensional observation coordinates on the image plane; the tangent space construction module is used to convert the two-dimensional observation coordinates into unit observation ray vectors and construct an orthogonal tangent space basis perpendicular to the ray. The residual and derivative calculation module is used to obtain the three-dimensional feature point vector to be estimated, project it onto the orthogonal tangent space basis to calculate the orthogonal ray projection residual, and calculate the Jacobian matrix of the residual with respect to the optimization variables, including the camera pose and the coordinates of the three-dimensional feature points; the state estimation module is used to determine the covariance matrix of the observation noise, construct a probabilistic state estimation model based on the residual, the Jacobian matrix and the covariance matrix, and solve the model to update the camera pose and the coordinates of the three-dimensional feature points, thereby completing the visual measurement.

Images