A device pose estimation method and system based on stereo images

Abstract

The application discloses a device pose estimation method and system based on stereo images, and the method comprises the following steps: acquiring a synchronous stereo image pair and performing stereo correction; extracting feature information of left and right images and performing matching and screening to obtain effective matching point pairs; calculating spatial three-dimensional point information through triangulation, and removing invalid points to form an effective three-dimensional space point set; calculating a pose transformation matrix based on the effective three-dimensional space point set and the corresponding relationship of adjacent frame features; and updating the absolute pose based on a pose recursive model and outputting the result. The application also comprises robust processing steps such as pose quality evaluation, state classification management and adaptive update strategy selection. The application enhances the robustness of pose estimation in a complex environment, and improves the continuity and stability of pose estimation.

Description

Technical Field

[0001] This invention belongs to the field of device sensing technology, specifically relating to a device pose estimation method and system based on stereo images. Background Technology

[0002] Pose estimation is a key technology in the autonomous motion and environmental perception of devices (such as robots). Its core objective is to determine the device's position and orientation in space in real time based on information acquired by sensors. In current engineering applications, visual sensors are widely used in pose estimation systems due to their low cost and rich information content.

[0003] Existing visual pose estimation methods mainly include monocular vision methods and stereo vision methods. Monocular vision methods typically rely on the motion relationship between consecutive images for pose estimation. Due to the lack of true scale information, they often require additional prior assumptions or other sensors for assistance, and their stability is poor in complex environments or under rapid motion conditions.

[0004] Stereo vision methods can directly recover the spatial depth of a scene by using the parallax information between the left and right viewpoints. Theoretically, they have the advantages of consistent scale and high accuracy, and therefore have received increasing attention and application in fields such as robotics, automated inspection and unmanned systems.

[0005] As robot applications develop towards embedded, miniaturized, and complex environments, and actual operating scenarios extend to complex environments with variable lighting, missing textures, and partial occlusion, the industry has put forward higher technical requirements for device pose estimation systems based on stereo images. That is, device pose estimation systems based on stereo images not only need to have high accuracy, but also need to take into account real-time performance, stability, and ease of engineering deployment.

[0006] Although existing methods for device pose estimation based on stereo images have made some progress in both theory and application, they still have the following shortcomings in practical engineering applications:

[0007] 1. High dependence on feature quality and insufficient robustness: In practical application environments, factors such as changes in illumination, missing textures, or local occlusion can lead to unstable feature extraction or feature matching results. Existing systems often lack effective data filtering or anomaly handling mechanisms when feature anomalies occur, which can easily cause jumps or even interruptions in pose estimation results.

[0008] 2. Insufficient continuity and stability of pose estimation, unable to meet the needs of continuous device motion: When feature information is missing or data quality deteriorates, existing technologies lack effective pose estimation maintenance strategies, which can easily lead to interruptions or jumps in pose results. This makes it impossible to provide stable and continuous pose information support for the continuous autonomous motion of the device, thus restricting its application in high-precision motion control, path planning and other scenarios.

[0009] 3. High computational complexity and strong environment dependence make it difficult to balance real-time performance with project deployability.

[0010] Some existing solutions have high computational complexity, making it difficult to meet the real-time requirements of pose estimation on resource-constrained platforms such as embedded devices and small robots. Furthermore, existing systems are highly dependent on the operating environment and hardware conditions, increasing the difficulty of deployment and debugging on different device platforms and hindering their widespread application in practical applications.

[0011] Therefore, there is a need to provide a device pose estimation method and system based on stereo images that has robust processing capabilities and is suitable for engineering deployment, in order to overcome the above-mentioned shortcomings. Summary of the Invention

[0012] The purpose of this application is to provide a device pose estimation method and system based on stereo images, which improves the robustness of pose estimation.

[0013] The technical solution provided in this application is as follows:

[0014] In a first aspect, this application provides a device pose estimation method based on stereo images, including:

[0015] S1. Stereo Image Acquisition: Acquire the left and right images simultaneously captured by the left and right cameras of the stereo vision sensor on the device to form a stereo image pair;

[0016] S2. Stereo Correction Processing: Based on the pre-obtained camera calibration parameters, stereo correction processing is performed on the stereo image pair to ensure that the left and right images satisfy the epipolar alignment relationship.

[0017] S3. Feature information extraction: Extract feature information from the corrected left and right images respectively. The feature information includes the position of the feature points and the descriptive information corresponding to the feature points.

[0018] S4. Left and right feature matching: Calculate the similarity of feature point description information in the left and right images, determine matching point pairs, and filter the matching point pairs for validity, retaining valid matching point pairs that meet the preset matching conditions.

[0019] S5. Spatial point calculation: Based on the effective matching point pairs and camera projection relationship, triangulation calculation is performed on the matching point pairs to obtain spatial point information. Spatial point validity constraints are introduced to eliminate invalid spatial points and form a set of effective three-dimensional spatial points.

[0020] S6. Pose Transformation Matrix Calculation: Based on the effective set of three-dimensional spatial points at the current moment and the correspondence between image features at adjacent moments, calculate the pose transformation matrix of the device at the current moment relative to the previous moment.

[0021] S7. Pose State Update and Output: Based on the pose recursive model, update the absolute pose of the device in the world coordinate system at the current moment according to the pose transformation matrix, and output the pose estimation result.

[0022] In one possible implementation, in S5, the camera projection relationship is as follows: ,in These are the two-dimensional pixel coordinates of a point in space within the image plane. This is the camera projection function from 3D spatial coordinates to 2D pixel coordinates. For the camera intrinsic parameter matrix, Here are the 3D coordinates of the spatial point in the camera coordinate system; the 3D coordinates of the spatial point in the camera coordinate system based on the left camera are: , ,in, and These represent the camera's focal length in the horizontal and vertical directions, respectively. and These represent the coordinates of the principal point, and These are the horizontal pixel coordinates of corresponding feature points in the left and right images, respectively; based on the principle of binocular geometry, the depth information of spatial points is calculated from disparity: ,in This represents the depth value of a point in the spatial coordinate system. The physical distance between the optical centers of the left and right cameras. This refers to the disparity of corresponding feature points in the left and right images after stereo correction. ; , and Together they constitute the three-dimensional coordinates of a point in space. .

[0023] In one possible implementation, in S5, the spatial point validity constraint is: when the disparity value of a spatial point is lower than a preset threshold or the calculated depth value exceeds a preset range, the spatial point is determined to be an invalid point and is removed.

[0024] In one possible implementation, step S6, calculating the pose transformation matrix of the device at the current moment relative to the previous moment, specifically includes:

[0025] S6.1 Camera Pose Representation: The device pose is represented by the pose of the camera in the world coordinate system using its onboard stereo vision sensor. The camera pose in the world coordinate system is expressed by a homogeneous transformation matrix. express, For rotation matrix, The vector is the translation vector; the pose transformation matrix of the device at the current moment relative to the previous moment, that is, the pose transformation matrix of the current frame relative to the previous frame is... , For the first Frame relative to the first The frame rotation matrix, For the first Frame relative to the first The translation vector of the frame;

[0026] S6.2, Pose Prediction Model Establishment: Establish the spatial points of the previous frame... Projecting the image onto the current frame yields the theoretical projected pixel positions, and the model expression is: , This is the camera projection function from 3D spatial coordinates to 2D pixel coordinates. express The theoretical projection pixel position in the current frame image;

[0027] S6.3 Temporal Feature Association and Reference Perspective Construction: Temporal feature matching relationships are constructed in the left and right image sequences respectively to obtain temporal feature matching results; based on the temporal feature matching results, a reference perspective is adaptively selected.

[0028] S6.4, Construction of 2D-3D Correspondence: Based on the temporal feature matching results, the spatial points of the previous frame are... The actual observed position of the corresponding feature point in the reference viewpoint image of the current frame Perform associations to construct a set of corresponding two-dimensional and three-dimensional point pairs. ;

[0029] S6.5 Solving for the pose transformation matrix: Construct a pose estimation model with the objective of minimizing the sum of squared reprojection errors, and optimize the solution to obtain the pose transformation matrix. The objective function is: , ,in express The reprojection error.

[0030] In one possible implementation, in S7, the pose recursion model is: ,in For the first The absolute pose matrix of the frame camera in the world coordinate system Indicates the first The absolute pose matrix of the frame camera in the world coordinate system; the global pose recursion relationship is as follows: , This is the initial pose matrix of the camera in the initial frame.

[0031] In one possible implementation, after attempting to solve the pose transformation matrix, the quality of the solution is evaluated, with the following evaluation dimensions:

[0032] Whether the solution process was successfully completed: If the solution fails due to reasons such as numerical non-convergence, insufficient number of corresponding point pairs, or matrix rank deficiency, it is directly considered that there is no valid pose solution;

[0033] Pose jump constraint: If the solution is successful, and the rotation angle or translation distance between the pose calculated in the current frame and the effective pose in the previous frame exceeds the preset threshold, then an abnormal jump is determined, and the pose solution is invalid.

[0034] Continuity constraint: If the solution is successful, but the deviation between the pose change rate of the current frame and the pose change rate of the previous few frames exceeds the preset range, then the continuity constraint is not satisfied and the pose solution is invalid.

[0035] A pose solution is considered valid only if the solution process is successfully completed and both pose jump and continuity constraints are satisfied; otherwise, it is considered that there is no valid pose solution.

[0036] In one possible implementation, based on the quality assessment results and combined with the number and distribution of corresponding point pairs in two-dimensional and three-dimensional dimensions, the pose estimation state at the current moment is managed hierarchically. The pose estimation state includes at least:

[0037] Normal state: Number of corresponding point pairs in two-dimensional and three-dimensional modes And spatial distribution uniformity The pose solution was successful, and the solution exhibited no abnormal jumps and satisfied the continuity constraint; among other things, The threshold for the number of corresponding point pairs. The threshold for the first spatial distribution uniformity;

[0038] Degenerate state: and The pose solution was successful but had minor anomalies; the pose solution did not exhibit drastic jumps and the continuity constraints were largely satisfied. The threshold for the number of corresponding point pairs. The second spatial distribution uniformity threshold;

[0039] Severe degradation state: or The pose solution fails, or the pose solution has drastic jumps or seriously fails to meet the continuity constraints.

[0040] In one possible implementation, a pose update strategy selection step based on the pose estimation state is also included, where a corresponding update strategy is adopted according to different pose estimation states:

[0041] Normal state: Using Update pose;

[0042] Degenerate state: Restrict or deweight pose updates, and use a weighted fusion method. Update the pose, or update only the translation component while keeping the rotation component unchanged, where As weight, , Adjust dynamically according to the degree of degradation;

[0043] Severe degradation state: maintaining the pose result from the previous moment. Alternatively, switch to an alternative reference viewpoint and solve again; if multiple switching attempts are ineffective, enter the pure prediction mode based on the kinematic model; and re-execute the pose update after the image frames return to normal.

[0044] Secondly, this application provides a device pose estimation system based on stereo images, used to perform the above-described device pose estimation method based on stereo images, the system comprising:

[0045] The image acquisition module is used to acquire the left and right images simultaneously captured by the left and right cameras of the stereo vision sensor on the device.

[0046] The stereo correction module is used to perform stereo correction processing on stereo image pairs based on pre-obtained camera calibration parameters, so that the left and right images meet the epipolar alignment relationship;

[0047] The feature extraction module is used to extract feature information from the corrected left and right images, respectively. The feature information includes the position of the feature points and the descriptive information corresponding to the feature points.

[0048] The feature matching module is used to calculate the similarity of feature point description information in the left and right images, determine matching point pairs, and filter the matching point pairs for validity, retaining valid matching point pairs that meet the preset matching conditions.

[0049] The triangulation module is used to perform triangulation calculations on the matching point pairs based on the valid matching point pairs and the camera projection relationship, obtain spatial point information, introduce spatial point validity constraints, eliminate invalid spatial points, and form a valid set of three-dimensional spatial points.

[0050] The pose estimation module is used to calculate the pose transformation matrix of the device at the current time relative to the previous time based on the effective set of three-dimensional spatial points at the current time and the correspondence between image features at adjacent time times. It is also used to update the absolute pose of the device at the current time in the world coordinate system based on the pose recursive model.

[0051] The robust processing module is used to detect abnormal situations in the pose estimation process, trigger the robust processing mechanism, filter or process abnormal data, and also to perform pose solution quality assessment, pose estimation state discrimination and hierarchical management, and pose update strategy selection.

[0052] The results output module is used to output the pose estimation results of the device in the world coordinate system at the current moment.

[0053] Thirdly, this application provides an electronic device, including: a memory and a processor;

[0054] The memory is used to store computer programs;

[0055] The processor is used to invoke the computer program to execute the method described above.

[0056] Fourthly, this application provides a computer-readable storage medium storing a computer program that, when executed on an electronic device, causes the electronic device to perform the method described above.

[0057] Fifthly, this application provides a computer program product, including a computer program that, when run on an electronic device, causes the electronic device to perform the method described above.

[0058] The specific implementation of the fifth aspect of this application can refer to the implementation of the first aspect mentioned above, and will not be elaborated here.

[0059] Beneficial effects:

[0060] Compared with the prior art, the device pose estimation method and system based on stereo images provided by the present invention have at least the following beneficial effects:

[0061] (1) Enhance the robustness of pose estimation in complex environments

[0062] This invention introduces anomaly data filtering and robust processing mechanisms in key steps such as feature matching, triangulation calculation, and pose estimation. This can effectively reduce the impact of factors such as mismatch and depth anomalies on pose estimation results, enabling the system to maintain stable operation even in complex environments such as changes in lighting, insufficient texture, or local occlusion.

[0063] (2) Improve the continuity and stability of pose estimation results

[0064] By properly handling abnormal situations, this invention can still maintain the continuous execution of the pose estimation process even when some feature information is missing or the data quality deteriorates, avoiding drastic jumps or interruptions in the pose results and improving the reliability of the system in practical applications.

[0065] (3) Applicable to resource-constrained computing platforms, balancing real-time performance and practicality.

[0066] The method of the present invention does not rely on a fixed and complex computing structure during implementation. It can select an appropriate implementation method according to the actual computing power conditions, reduce the computing burden while ensuring the accuracy of pose estimation, and is suitable for resource-constrained platforms such as embedded devices and small robots.

[0067] (4) Improve the clarity of the system structure and reduce the cost of engineering implementation and maintenance.

[0068] This invention modularizes the stereo image pose estimation process, separating functions such as image acquisition, stereo correction, feature processing, spatial calculation, and pose estimation. Each module has a clear responsibility and is independent of the others, avoiding the problem of high coupling of multiple functions in the prior art, which is beneficial to the engineering implementation, debugging, and later maintenance of the system.

[0069] Through the above-mentioned technical effects, the present invention is superior to the prior art in terms of engineering feasibility, system stability and practical application value. Attached Figure Description

[0070] Figure 1 This is a flowchart of a method in one embodiment of this application;

[0071] Figure 2 This is a flowchart of the pose update strategy selection steps based on pose estimation state in one embodiment of this application;

[0072] Figure 3 This is a system framework diagram of one embodiment of this application. Detailed Implementation

[0073] To enable those skilled in the art to better understand the present application, the technical solution of the present application will be further described in detail below with reference to the embodiments and accompanying drawings.

[0074] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this application are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate for the embodiments of this application described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0075] Example 1:

[0076] This application provides a device pose estimation method based on stereo images. By organizing the stereo image processing flow into steps and introducing robust processing mechanisms at key processing stages, stable and continuous pose estimation is achieved. This method is suitable for the pose calculation needs of robots and other mobile devices in complex environments. This application involves three fundamental coordinate systems: the world coordinate system {W}, the camera coordinate system {C}, and the pixel coordinate system {I}, which uniformly describe the mapping relationship between spatial points, camera positions, and image observations. The definitions of each coordinate system are as follows:

[0077] The world coordinate system {W} is used to describe the positional relationship of the observed environment or robot in global space.

[0078] The camera coordinate system {C} has the camera optical center as its origin, and its coordinate axes are aligned with the camera mounting direction. It is used to describe the three-dimensional position of a point in space from the camera's perspective.

[0079] The pixel coordinate system {I} takes the top left corner of the image plane as its origin and uses pixel coordinates to represent the position of pixels in the image plane.

[0080] Spatial points can be mapped between different coordinate systems through coordinate transformation relationships.

[0081] The device pose estimation method based on stereo images includes the following steps:

[0082] Step S1: Stereo Image Acquisition

[0083] The device acquires left and right images simultaneously captured by the left and right cameras of the stereo vision sensor, which together form a stereo image pair.

[0084] The left and right images are acquired simultaneously using a stereo vision sensor.

[0085] The left and right images are kept synchronized in time to avoid errors in subsequent spatial calculations based on the binocular geometric model due to time deviation, and to provide basic image observation data for subsequent 3D point reconstruction and pose solving.

[0086] Step S2: Stereoscopic correction processing

[0087] Based on the pre-obtained camera calibration parameters, the stereo image pair is subjected to stereo correction processing to ensure that the left and right images satisfy the epipolar alignment relationship.

[0088] In some embodiments, a pinhole camera model is used as a camera imaging model to describe the imaging process of spatial points.

[0089] Camera calibration parameters, including the camera intrinsic parameter matrix ;in, , These represent the camera's focal length in the horizontal and vertical directions, respectively. , Indicates the coordinates of the principal point.

[0090] In a stereo vision system, there is a fixed spatial baseline relationship between the left and right cameras. After stereo correction of the left and right images, the images satisfy the epipolar alignment condition, and the corresponding feature points of the same spatial point in the left and right images have a disparity relationship. Let the corresponding feature points be: ,in and These represent the horizontal and vertical pixel coordinates of the feature point in the left image, respectively, in the pixel coordinate system. and Let the horizontal and vertical pixel coordinates of the corresponding feature points in the right image be represented respectively; then the disparity is defined as: After stereo correction, the left and right images satisfy the epipolar alignment condition. The pixel coordinates of corresponding feature points in the corrected left and right images only have parallax in the horizontal direction (vertical pixel coordinate offset is negligible), that is, the vertical offset of the corresponding pixel points. The disparity can be approximated as negligible, therefore it can be simplified to: This significantly reduces the search complexity of subsequent feature matching and improves matching reliability, laying the foundation for the efficient application of binocular geometric models.

[0091] Step S3: Feature Information Extraction

[0092] Feature information is extracted from the corrected left and right images respectively. The feature information includes the position of the feature point (the pixel coordinates of the feature point in the pixel coordinate system) and the descriptive information corresponding to the feature point.

[0093] In the technical framework of this solution, a feature point is a two-dimensional coordinate position (u,v) in the pixel coordinate system, while the descriptive information corresponding to the feature point is a feature vector / feature string obtained by digitizing and standardizing the local visual features of the extracted image feature point. It is a digital identity of the feature point. It is generated based on visual information such as the pixel grayscale, texture, gradient direction / magnitude, and neighborhood pixel distribution around the feature point, and has uniqueness, distinguishability, and invariance (robust to scale, rotation, and illumination changes).

[0094] The feature extraction method can be selected based on computing resources to ensure feature stability while meeting the real-time requirements of the system. This application does not limit the specific feature extraction or matching implementation method and can be flexibly configured according to different application scenarios and hardware conditions, facilitating its widespread application in various robot systems or visual perception systems.

[0095] Step S4: Left and right feature matching

[0096] By calculating the similarity (such as Euclidean distance or Hamming distance) between the description information of each feature point in the left image and the description information of all feature points in the right image, the pair of feature points with the highest similarity is determined as the matching point pair, thus achieving accurate pairing of feature points of the same spatial point in the left and right images.

[0097] To reduce the impact of mismatches on subsequent pose estimation, the matching point pairs can be filtered for validity, retaining only those matching point pairs that meet preset matching conditions (such as matching similarity higher than a preset threshold, disparity within a reasonable range, and no duplicate matching / many-to-one matching).

[0098] Step S5: Spatial point calculation (recovery)

[0099] Based on the valid matching point pairs and camera projection relationship, triangulation calculation is performed on the matching point pairs to obtain the corresponding spatial point information.

[0100] The camera projection relationship, that is, the projection relationship of spatial points in the image plane, can be expressed as: ;in, Represents the two-dimensional pixel coordinates of a point in space within the image plane, such as , indicating the actual observed location of a spatial point on the image; A camera projection function representing the projection from three-dimensional spatial coordinates to two-dimensional pixel coordinates; This is the camera intrinsic parameter matrix, describing the camera's internal optical and geometric parameters. Its standard form is:

[0101] ;

[0102] in, and These are the camera's focal lengths in the horizontal and vertical directions, respectively (determined by the camera's parameters). and These are the pixel coordinates of the principal point of the image (the intersection of the optical axis and the image plane); The three-dimensional coordinates of a point in space in the camera coordinate system are in the form of: This describes the position of a point in a local coordinate system with the camera's optical center as the origin.

[0103] Based on the principles of binocular geometry (stereoscopic geometric model), the depth information of spatial points can be calculated from disparity:

[0104] ;

[0105] in, This is the depth value of a spatial point in the camera coordinate system, that is, the distance from the spatial point to the optical center of the camera; The baseline distance, which is the physical distance between the optical centers of the left and right cameras, can be obtained through camera calibration.

[0106] Furthermore, the three-dimensional coordinates of a spatial point in a camera coordinate system based on the left camera (with the optical center of the left camera as the origin of the camera coordinate system, and the mounting orientation of the left camera defining the three axes of the coordinate system) can be expressed as:

[0107] ;

[0108] in, and Let be the three-dimensional coordinate components of a point in the camera coordinate system based on the left camera, and the depth. Together they form a complete three-dimensional coordinate system. .

[0109] Using the above model, a set of spatial points can be recovered from a stereo image.

[0110] In the Within the frame, the set of spatial points is recovered through stereo triangulation. .

[0111] In actual imaging processes, some disparity values ​​may become abnormal due to factors such as image noise, matching errors, or changes in illumination. To ensure the reliability of spatial points and improve the reliability of 3D reconstruction results, this invention introduces a spatial point validity constraint: when the disparity value of a spatial point is lower than a preset threshold or the depth value calculated from it exceeds a preset range, the spatial point is determined to be an invalid point (abnormal or unreliable point) and is removed, forming a set of valid 3D spatial points at the current moment, so as to avoid abnormal data interfering with subsequent pose estimation results.

[0112] Step S6: Calculation of pose transformation matrix

[0113] Based on the spatial point information obtained at the current moment and the correspondence between image features from adjacent moments, the pose information of the device at the current moment is calculated. This pose information describes the position and orientation of the device relative to a reference coordinate system.

[0114] S6.1 Camera Pose Representation: The device pose is represented by the pose of the camera of its onboard stereo vision sensor in the world coordinate system. The camera pose in the world coordinate system is represented by a homogeneous transformation matrix, which is a combination of a rotation matrix and a translation vector. The expression is:

[0115] ;

[0116] in, It is a homogeneous transformation matrix. The rotation matrix describes the camera pose. This is the translation vector describing the camera's position.

[0117] Let the spatial point in the world coordinate system be... Then the spatial point is represented in the camera coordinate system as: .

[0118] S6.2, Establishment of pose prediction model (observation model):

[0119] In a continuous image sequence, there are pose changes between adjacent time points of the camera.

[0120] Let the current frame (the first frame) be... The current frame (relative to the previous frame) is the frame (the current time). The pose transformation matrix of the previous frame is: The expression is:

[0121] ;

[0122] in, For the first Frame relative to the first The frame rotation matrix, No. Frame relative to the first The translation vector of the frame.

[0123] Let the first Frame recovery yields the first The spatial points are .

[0124] When the camera moves to the current frame, based on the pose prediction model, the spatial points of the previous frame are projected onto the current frame image to obtain the theoretical projected pixel positions; the expression of the pose prediction model is: ,in express The theoretical projection pixel position in the current frame image.

[0125] S6.3, Temporal Feature Correlation and Reference Perspective Construction:

[0126] A temporal feature correlation is constructed between the current frame and the previous frame to describe the spatial consistency between adjacent frames.

[0127] Specifically, temporal feature matching relationships can be constructed in the left and right image sequences respectively, and the number of corresponding effective features can be counted. Among them, effective features refer to feature points that successfully establish temporal matching relationships and satisfy the validity constraints within the same viewpoint (left / right individual viewpoint) of adjacent frames (such as frame k-1 and frame k). The judgment conditions are as follows: the similarity of the feature point description information between adjacent frames is higher than the preset temporal matching threshold, and the matching relationship is unique.

[0128] In temporal feature matching from a certain perspective (left / right), if the number of valid features obtained is greater than or equal to the preset matching number threshold, then the perspective is determined to obtain a valid temporal feature matching result; if the number of valid features is less than the preset matching number threshold, then the perspective is determined to not obtain a valid temporal feature matching result.

[0129] If effective temporal feature matching results can be obtained from both left and right perspectives, the temporal matching quality of both sides (the evaluation dimensions of matching quality may include, but are not limited to, the number of effective features, the spatial distribution uniformity of effective features, the magnitude of the reprojection error of feature matching, the stability of matching point pairs, etc.) is evaluated, and the side with higher matching quality is selected as the reference perspective at the current moment.

[0130] If a single perspective can yield effective temporal feature matching results, that perspective should be directly selected as the reference perspective for the current moment.

[0131] If no effective temporal matching result can be obtained from either the left or right viewpoints, the current pose estimation is determined to have entered a severely degraded state, triggering the corresponding robust processing strategy.

[0132] By using the above method, the reference viewpoint for pose estimation can be adaptively selected according to changes in image quality, so as to avoid estimation instability caused by unilateral occlusion, reflection or texture degradation.

[0133] S6.4 Construction of 2D-3D Correspondence:

[0134] After determining the reference viewpoint, based on the temporal feature matching results (the correspondence between spatial points and image features between the current time and adjacent time points), the spatial points of the previous frame are... The actual observed position of the corresponding feature point in the reference viewpoint image of the current frame By associating them, a set of corresponding two-dimensional and three-dimensional point pairs can be constructed. .

[0135] S6.5 Solving for the pose transformation matrix:

[0136] When the two-dimensional-three-dimensional correspondence satisfies the pose solution conditions, a pose estimation model is constructed based on the corresponding point pairs to solve the pose transformation of the camera or device at the current moment relative to the previous moment.

[0137] In some embodiments, the objective function of the pose estimation model is:

[0138] ;

[0139] ;

[0140] in, Representing a spatial point Reprojection error; This represents the sum of squared reprojection errors for all valid spatial points.

[0141] By optimizing the above objective function, the current frame (the first frame) is obtained. (frame) relative to the previous frame (the first frame) pose transformation matrix of frame .

[0142] Step S8: Pose state update and output;

[0143] Pose Recursion Model: No. Frame camera based The pose of a frame in world coordinates can be represented as:

[0144] ;

[0145] in, Indicates the first The absolute pose matrix of the frame camera in the world coordinate system (homogeneous transformation matrix, 4×4).

[0146] Indicates the first The absolute pose matrix of the frame camera in the world coordinate system.

[0147] Globally, we have: ,like At that time, ,in, This represents the initial pose matrix of the camera in the world coordinate system at frame 0 (initial frame); Represents the identity matrix.

[0148] This pose recursive model can describe the continuous pose changes of the camera during motion.

[0149] Therefore, based on the pose recursive model Update the current pose state of the system.

[0150] Output the pose estimation result at the current moment. This result is the position and attitude information of the device in the world coordinate system. It can be output at a preset frequency and used for subsequent functional modules such as motion control, path planning, state recording or environmental perception. It provides continuous and stable pose information support for the autonomous movement of robots, unmanned systems and the like.

[0151] In some embodiments, the method further includes:

[0152] After attempting to solve the pose transformation matrix, the quality of the solution is evaluated, and the evaluation dimensions are as follows:

[0153] Whether the solution process was successfully completed: If the solution fails due to reasons such as numerical non-convergence, insufficient number of corresponding point pairs, or matrix rank deficiency (no effective pose transformation matrix is ​​obtained), it is directly considered that there is no effective pose solution;

[0154] Pose jump constraint: If the solution is successful, if the rotation angle or translation distance between the pose calculated in the current frame and the effective pose in the previous frame exceeds the preset threshold (which can be set according to the application scenario, such as rotation angle greater than 5° or translation distance greater than 0.5m), then it is determined that there is an abnormal jump and the pose solution is invalid.

[0155] Continuity constraint: If the solution is successful, but the deviation between the pose change rate of the current frame and the pose change rate of the previous few frames exceeds a preset range (e.g., acceleration exceeds 3σ), then the continuity constraint is not satisfied and the pose solution is invalid.

[0156] A pose solution is considered valid only if the solution process is successfully completed and both pose jump and continuity constraints are satisfied; otherwise, it is considered that there is no valid pose solution.

[0157] In some embodiments, the method further includes a pose estimation state discrimination and hierarchical management step;

[0158] Based on the above solution quality assessment results, and combined with the number and distribution of corresponding point pairs in 2D and 3D, the pose estimation state at the current moment is managed hierarchically to improve the robustness of the system in complex scenarios. The pose estimation state includes at least:

[0159] Normal state; its determination criteria include: in the two-dimensional-three-dimensional correspondence construction step, the number of two-dimensional-three-dimensional correspondence point pairs is sufficient and the spatial distribution is uniform. For example, ,in This represents the number of corresponding point pairs in two-dimensional and three-dimensional space. The threshold for the number of first corresponding point pairs, spatial distribution uniformity. ,in The first spatial distribution uniformity threshold is set; the pose solution process is successfully completed; the pose solution has no abnormal jumps and satisfies the continuity constraint. For example, the rotation angle between the pose of the current frame and the pose of the previous frame is set. Translation distance ,in The first rotation angle threshold, This represents the first translation distance threshold. A normal state indicates that the current image observation and pose estimation process is stable, and the current solution result is reliable.

[0160] Degenerate state; its determination criteria include: the number of corresponding point pairs in two-dimensional and three-dimensional dimensions is slightly less than (like , (The threshold for the number of corresponding point pairs), and the spatial distribution uniformity is slightly lower than (like , (The second spatial distribution uniformity threshold) is still acceptable; the pose solution process is successful, but there are slight anomalies (such as a slightly higher reprojection error, etc.). ,in This represents the average reprojection error. and (These are the first and second average reprojection error thresholds, respectively); the pose solution does not have drastic jumps, and the continuity constraints are basically satisfied, such as the rotation angle between the pose of the current frame and the previous frame. Translation distance ,in The second rotation angle threshold, This represents the second translation distance threshold. The degraded state indicates decreased observation conditions but still allows for limited pose information; the reliability of the results is reduced, but their usability is limited.

[0161] Severe degradation state; its judgment criteria include: no effective temporal feature matching results can be obtained from both perspectives; the number of two-dimensional-three-dimensional corresponding point pairs is severely insufficient (e.g. ) or distribution range (e.g. Unable to solve; pose calculation process failed (due to numerical non-convergence, matrix rank deficiency, or reprojection error). Failure due to reasons such as (e.g., the pose solution has drastic jumps or seriously fails to meet continuity constraints). or A severely degraded state indicates that reliable pose estimation conditions are not available at the current moment, and the current solution result is unreliable.

[0162] The threshold parameters can be set based on experience.

[0163] Based on the above state determination results, the corresponding degradation processing strategy will be executed:

[0164] In some embodiments, the method further includes:

[0165] Based on the current pose estimation state, different pose update strategies are adopted to avoid abnormal pose solutions from damaging the overall stability of the system. Specifically, these include:

[0166] When in a normal state, the pose is updated using the pose result obtained from the current solution. This ensures that high-quality observations are fully utilized to guarantee the real-time performance and accuracy of pose updates.

[0167] When in a degenerate state, the pose update process can be restricted or downweighted, for example, by using a weighted fusion method to update the pose: ,in For weights ( (It can be dynamically adjusted according to the degree of degradation), or only the translation component can be updated while the rotation component remains unchanged. This reduces the impact of errors from low-quality observations on the system state and maintains the smoothness of pose estimation.

[0168] When in a severely degraded state, the pose result from the previous moment remains unchanged. If no valid result is obtained after multiple switches, the system enters pure prediction mode (predicting pose based on the kinematic model). This isolates unreliable observations to prevent abnormal pose solutions from disrupting the overall stability of the system. Pose updates are re-executed once the image frames return to normal.

[0169] After completing the pose update strategy selection, output the pose result at the current moment and use it as the initial state for the pose estimation at the next moment.

[0170] In some embodiments, the pose estimation process is continuously executed at a preset frequency, and the pose result calculated at each time step is output.

[0171] By repeatedly performing the above steps in a continuous sequence of images, continuous pose estimation based on stereo images can be achieved.

[0172] In some embodiments, such as Figure 2 As shown, the pose update strategy is dynamically adjusted based on the number of valid matches and the number of consecutive abnormal frames to ensure the system's stability in complex environments. The process includes:

[0173] Count the number of two-dimensional-three-dimensional corresponding point pairs (valid matches) in the current frame that are successfully associated with spatial points in the previous time step and feature points in the current image. ,like If the pose update is successful, the system enters the normal state and performs normal pose updates; otherwise, it enters the degenerate state and performs deweighting or restriction on pose updates.

[0174] Under normal conditions, the pose is updated using the currently solved pose result, and then the number of consecutive abnormal frames is checked. If the number of consecutive abnormal frames is... If K is selected, the observation quality is considered to be continuously deteriorating and entering a severely degraded state. The strategy of "keeping the pose of the previous frame" or "switching the reference viewpoint" is executed. Otherwise, the current pose update is considered to be valid, and the pose result is directly output.

[0175] In the degenerate state, the pose update process is restricted or downweighted, and then the number of consecutive abnormal frames is checked. If the number of consecutive abnormal frames is... K considers that the observation conditions of the current reference viewpoint cannot be recovered and executes "keep the pose of the previous frame". Otherwise, it attempts to obtain effective observations from the image on the other side by "switching the reference viewpoint" in order to restore the stability of the pose estimation.

[0176] When entering a severely degraded state, the most conservative strategy is directly implemented: maintain the pose of the previous frame to avoid erroneous poses from damaging the system state; or switch the reference viewpoint to attempt to recover valid observations from other perspectives. Furthermore, a multi-dimensional recovery attempt can be triggered, waiting for the return of valid observation conditions. That is, while maintaining the pose of the previous frame, the observation recovery mechanism is simultaneously initiated to attempt to reacquire valid temporal matching results, specifically including:

[0177] Re-execute the feature extraction and matching process: re-extract features from the left and right images of the current frame, match left and right features, and associate time-series features to eliminate matching failures caused by single image noise and temporary occlusion;

[0178] Expand the threshold range for feature matching: While ensuring matching reliability, appropriately relax the preset thresholds for feature similarity and the number of effective features to try to obtain more effective matching point pairs;

[0179] Continuously monitor image observation quality: Repeat the above operations at the system preset frequency until a valid temporal matching result can be obtained again from one or both sides of the viewpoint, and the normal reference viewpoint selection and pose solution process is restored.

[0180] In actual operation, environmental factors such as changes in illumination, insufficient texture, local occlusion, and image blurring can easily cause feature information extraction or matching failures, leading to problems such as insufficient matching points and abnormal spatial point distribution. To address these issues, the above embodiments of this application introduce a robust processing mechanism based on the three-dimensional reconstruction model and pose estimation principle of stereo images. This mechanism enables robust pose estimation for continuous image sequences, thereby improving the stability and continuity of the pose estimation process under complex imaging conditions. The robust processing mechanism first judges and handles abnormal scenarios: when it detects an abnormal decrease in the number of feature matching points, abnormal spatial point distribution, discontinuous pose changes in consecutive frames, or a number of effective matching points lower than a preset threshold, it determines that the pose estimation result of the current frame is not reliable enough. If it detects an abnormal distribution of depth information obtained from spatial point recovery, it directly removes the abnormal spatial points to avoid abnormal data interfering with the pose estimation result. Simultaneously, when the above abnormal situations are triggered, the system pauses the pose update process of the current frame, maintains the pose state of the previous moment, and re-executes the pose update operation after subsequent image frames return to normal. Through the robust processing method described above, this scheme can achieve stable and reliable pose estimation under continuous image input conditions, fully meeting the needs of practical engineering applications.

[0181] Specifically, during the pose estimation process, based on the relationship between the pose estimation mathematical model and the temporal observation, abnormal data is robustly processed, including filtering out disparity anomalies, depth anomalies, or invalid matching points; and in the case of missing some feature information, the pose estimation process is kept running continuously to improve the stability of the system in complex environments.

[0182] Anomaly detection: When an abnormal decrease in the number of feature matching points, abnormal spatial point distribution, or discontinuous pose changes in consecutive frames are detected, a robust processing mechanism is triggered.

[0183] Reference viewpoint adaptive selection: Construct temporal feature correlation between adjacent frames, and adaptively select reference viewpoints based on the temporal matching quality of left and right viewpoints to avoid estimation instability caused by unilateral occlusion, reflection, etc.

[0184] Pose state hierarchical management: Based on data quality and pose solution stability, pose estimation states are divided into normal state, degenerate state, and severely degenerate state;

[0185] Adaptive update strategy: In normal state, the system state is updated using the current solved pose; in degenerate state, the update process is restricted or weighted; in severely degenerate state, the pose result of the previous moment is maintained, or the reference view is switched and the effective observation conditions are restored, so as to avoid abnormal pose solutions from destroying the overall stability of the system.

[0186] Furthermore, at each time step, spatial points are recovered based on the left and right images, and the pose estimation process is dynamically evaluated and adaptively scheduled by combining the temporal observation relationship between adjacent time steps.

[0187] It should be understood that the numbers S1 to S7 are only used to distinguish and facilitate the expression of different steps, and do not necessarily constitute a restriction on the execution order between the steps.

[0188] Example 2:

[0189] This application provides a device pose estimation system based on stereo images, used in the device pose estimation method based on stereo images described in Embodiment 1. The system includes:

[0190] The image acquisition module is used to acquire the left and right images simultaneously captured by the left and right cameras of the stereo vision sensor on the device.

[0191] The stereo correction module is used to perform stereo correction processing on stereo image pairs based on pre-obtained camera calibration parameters, so that the left and right images meet the epipolar alignment relationship;

[0192] The feature extraction module is used to extract feature information from the corrected left and right images, respectively. The feature information includes the position of the feature points and the descriptive information corresponding to the feature points.

[0193] The feature matching module is used to calculate the similarity of feature point description information in the left and right images, determine matching point pairs, and filter the matching point pairs for validity, retaining valid matching point pairs that meet the preset matching conditions.

[0194] The triangulation module is used to perform triangulation calculations on the matching point pairs based on the valid matching point pairs and the camera projection relationship, obtain spatial point information, introduce spatial point validity constraints, eliminate invalid spatial points, and form a valid set of three-dimensional spatial points.

[0195] The pose estimation module is used to calculate the pose transformation matrix of the device at the current time relative to the previous time based on the effective set of three-dimensional spatial points at the current time and the correspondence between image features at adjacent time times. It is also used to update the absolute pose of the device at the current time in the world coordinate system based on the pose recursive model.

[0196] The robust processing module is used to detect abnormal situations in the pose estimation process, trigger the robust processing mechanism, filter or process abnormal data, and also to perform pose solution quality assessment, pose estimation state discrimination and hierarchical management, and pose update strategy selection.

[0197] The results output module is used to output the pose estimation results of the device in the world coordinate system at the current moment.

[0198] In existing technologies, functions such as stereo image processing, feature extraction, feature matching, and pose calculation are often tightly coupled within the same processing flow, lacking clear module division. This structure is detrimental to system maintenance and upgrades in engineering implementation. When an anomaly occurs in one processing step, it can easily affect the overall pose estimation process, reducing system reliability. This application adopts a modular system architecture design, completely decoupling functions such as image acquisition, stereo correction, feature processing, triangulation, pose estimation, and robust processing. Each module communicates through a data interface and operates independently, without relying on specific hardware operating environments or hardware / software combinations. It can be flexibly deployed and adapted to different robot hardware platforms according to actual engineering scenarios, significantly reducing the system's specific dependence on hardware conditions and facilitating system deployment, maintenance, and expansion.

[0199] Example 3:

[0200] like Figure 3 As shown in the system block diagram, this application embodiment provides a device pose estimation system based on stereo images, including:

[0201] A stereo vision sensor is used to simultaneously acquire left and right images;

[0202] The processing unit is used to perform stereo image correction, feature processing, spatial calculation, and pose estimation.

[0203] The data interface is used to output pose estimation results for robot control or state recording.

[0204] The processing unit integrates a stereo correction module, a feature extraction module, a feature matching module, a triangulation module, a pose estimation module, and a robust processing module. Each module is executed sequentially according to a preset data processing order. The corrected image output by the stereo correction module serves as the input to the feature extraction module. The feature information output by the feature extraction module is passed to the feature matching module. The matching result generated by the feature matching module is passed to the triangulation module. The spatial point information output by the triangulation module is used for subsequent pose transformation matrix calculation.

[0205] Example 4:

[0206] This embodiment provides an electronic device, including: a memory and a processor;

[0207] The memory is used to store computer programs;

[0208] The processor is configured to invoke the computer program to execute the method as described in Embodiment 1.

[0209] Example 5:

[0210] This embodiment provides a computer-readable storage medium storing a computer program. When the computer program is run on an electronic device, it causes the electronic device to perform the method described in Embodiment 1.

[0211] Example 6:

[0212] This embodiment provides a computer program product, including a computer program that, when run on an electronic device, causes the electronic device to perform the method described in Embodiment 1.

[0213] The specific implementation methods of the systems, electronic devices, computer-readable storage media, and computer program products provided in the embodiments of this application can be referred to the specific embodiments of the above methods, and will not be repeated here.

[0214] Obviously, those skilled in the art should understand that the various units or steps of this application described above can be implemented using general-purpose computing devices. They can be centralized on a single computing device or distributed across a network of multiple computing devices. Optionally, they can be implemented using computer-executable program code, thereby storing them in a storage device for execution by a computing device, or fabricating them separately as individual integrated circuit modules, or fabricating multiple modules or steps into a single integrated circuit module. Thus, this application is not limited to any particular combination of hardware and software.

[0215] The above description is merely a preferred embodiment of this application and is not intended to limit this application. Various modifications and variations can be made to this application by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the protection scope of this application.

Claims

1. A device pose estimation method based on stereo images, characterized in that, include: S1. Stereo Image Acquisition: Acquire the left and right images simultaneously captured by the left and right cameras of the stereo vision sensor on the device to form a stereo image pair; S2. Stereo Correction Processing: Based on the pre-obtained camera calibration parameters, stereo correction processing is performed on the stereo image pair to ensure that the left and right images satisfy the epipolar alignment relationship. S3. Feature information extraction: Extract feature information from the corrected left and right images respectively. The feature information includes the position of the feature points and the descriptive information corresponding to the feature points. S4. Left and right feature matching: Calculate the similarity of feature point description information in the left and right images, determine matching point pairs, and filter the matching point pairs for validity, retaining valid matching point pairs that meet the preset matching conditions. S5. Spatial point calculation: Based on the effective matching point pairs and camera projection relationship, triangulation calculation is performed on the matching point pairs to obtain spatial point information. Spatial point validity constraints are introduced to eliminate invalid spatial points and form a set of effective three-dimensional spatial points. S6. Pose Transformation Matrix Calculation: Based on the effective set of three-dimensional spatial points at the current moment and the correspondence between image features at adjacent moments, calculate the pose transformation matrix of the device at the current moment relative to the previous moment. S7. Pose State Update and Output: Based on the pose recursive model, update the absolute pose of the device in the world coordinate system at the current moment according to the pose transformation matrix, and output the pose estimation result.

2. The device pose estimation method based on stereo images according to claim 1, characterized in that, In S5, the camera projection relationship is as follows: ,in These are the two-dimensional pixel coordinates of a point in space within the image plane. This is the camera projection function from 3D spatial coordinates to 2D pixel coordinates. For the camera intrinsic parameter matrix, Here are the 3D coordinates of the spatial point in the camera coordinate system; the 3D coordinates of the spatial point in the camera coordinate system based on the left camera are: , ,in, and These represent the camera's focal length in the horizontal and vertical directions, respectively. and These represent the coordinates of the principal point, and These are the horizontal pixel coordinates of corresponding feature points in the left and right images, respectively; based on the principle of binocular geometry, the depth information of spatial points is calculated from disparity: ,in This represents the depth value of a point in the spatial coordinate system. The physical distance between the optical centers of the left and right cameras. This refers to the disparity of corresponding feature points in the left and right images after stereo correction. ; , and Together they constitute the three-dimensional coordinates of a point in space. .

3. The device pose estimation method based on stereo images according to claim 1, characterized in that, In S5, the spatial point validity constraint is as follows: when the disparity value of a spatial point is lower than a preset threshold or the calculated depth value exceeds a preset range, the spatial point is determined to be an invalid point and is removed.

4. The device pose estimation method based on stereo images according to claim 1, characterized in that, In S6, calculating the pose transformation matrix of the device at the current moment relative to the previous moment specifically includes: S6.1 Camera Pose Representation: The device pose is represented by the pose of the camera in the world coordinate system using its onboard stereo vision sensor. The camera pose in the world coordinate system is expressed by a homogeneous transformation matrix. express, For rotation matrix, The vector is the translation vector; the pose transformation matrix of the device at the current moment relative to the previous moment, that is, the pose transformation matrix of the current frame relative to the previous frame is... , For the first Frame relative to the first The frame rotation matrix, For the first Frame relative to the first The translation vector of the frame; S6.2, Pose Prediction Model Establishment: Establish the spatial points of the previous frame... Projecting the image onto the current frame yields the theoretical projected pixel positions, and the model expression is: , This is the camera projection function from 3D spatial coordinates to 2D pixel coordinates. express The theoretical projection pixel position in the current frame image; S6.3 Temporal Feature Association and Reference Perspective Construction: Temporal feature matching relationships are constructed in the left and right image sequences respectively to obtain temporal feature matching results; based on the temporal feature matching results, a reference perspective is adaptively selected. S6.4, Construction of 2D-3D Correspondence: Based on the temporal feature matching results, the spatial points of the previous frame are... The actual observed position of the corresponding feature point in the reference viewpoint image of the current frame Perform associations to construct a set of corresponding two-dimensional and three-dimensional point pairs. ; S6.5 Solving for the pose transformation matrix: Construct a pose estimation model with the objective of minimizing the sum of squared reprojection errors, and optimize the solution to obtain the pose transformation matrix. The objective function is: , ,in express The reprojection error.

5. The device pose estimation method based on stereo images according to claim 4, characterized in that, In S7, the pose recursion model is as follows: ,in For the first The absolute pose matrix of the frame camera in the world coordinate system Indicates the first The absolute pose matrix of the frame camera in the world coordinate system; the global pose recursion relationship is as follows: , This is the initial pose matrix of the camera in the initial frame.

6. The device pose estimation method based on stereo images according to claim 1, characterized in that, After attempting to solve the pose transformation matrix, the quality of the solution is evaluated, and the evaluation dimensions are as follows: Whether the solution process was successfully completed: If the solution fails due to reasons such as numerical non-convergence, insufficient number of corresponding point pairs, or matrix rank deficiency, it is directly considered that there is no valid pose solution; Pose jump constraint: If the solution is successful, and the rotation angle or translation distance between the pose calculated in the current frame and the effective pose in the previous frame exceeds the preset threshold, then an abnormal jump is determined, and the pose solution is invalid. Continuity constraint: If the solution is successful, but the deviation between the pose change rate of the current frame and the pose change rate of the previous few frames exceeds the preset range, then the continuity constraint is not satisfied and the pose solution is invalid. A pose solution is considered valid only if the solution process is successfully completed and both pose jump and continuity constraints are satisfied; otherwise, it is considered that there is no valid pose solution.

7. The device pose estimation method based on stereo images according to claim 6, characterized in that, Based on the quality assessment results, and combined with the quantity and distribution of corresponding point pairs in two-dimensional and three-dimensional dimensions, the pose estimation state at the current moment is managed in a hierarchical manner. The pose estimation state includes at least the following: Normal state: Number of corresponding point pairs in two-dimensional and three-dimensional modes And spatial distribution uniformity The pose solution was successful, and the solution exhibited no abnormal jumps and satisfied the continuity constraint; among other things, The threshold for the number of corresponding point pairs. The threshold for the first spatial distribution uniformity; Degenerate state: and The pose solution was successful but had minor anomalies; the pose solution did not exhibit drastic jumps and the continuity constraints were largely satisfied. The threshold for the number of corresponding point pairs. The second spatial distribution uniformity threshold; Severe degradation state: or The pose solution fails, or the pose solution has drastic jumps or seriously fails to meet the continuity constraints.

8. The device pose estimation method based on stereo images according to claim 7, characterized in that, It also includes a pose update strategy selection step based on the pose estimation state, which adopts the corresponding update strategy according to different pose estimation states: Normal state: Using Update pose; Degenerate state: Restrict or deweight pose updates, and use a weighted fusion method. Update the pose, or update only the translation component while keeping the rotation component unchanged, where As weight, , Adjust dynamically according to the degree of degradation; Severe degradation state: maintaining the pose result from the previous moment. Alternatively, switch to an alternative reference perspective and solve again; If multiple switching attempts fail, the system will enter a pure prediction mode based on a kinematic model. Once the image frames return to normal, the pose update will be performed again.

9. A device pose estimation system based on stereo images, characterized in that, The system is used to perform the device pose estimation method based on stereo images according to any one of claims 1-8, the system comprising: The image acquisition module is used to acquire the left and right images simultaneously captured by the left and right cameras of the stereo vision sensor on the device. The stereo correction module is used to perform stereo correction processing on stereo image pairs based on pre-obtained camera calibration parameters, so that the left and right images meet the epipolar alignment relationship; The feature extraction module is used to extract feature information from the corrected left and right images, respectively. The feature information includes the position of the feature points and the descriptive information corresponding to the feature points. The feature matching module is used to calculate the similarity of feature point description information in the left and right images, determine matching point pairs, and filter the matching point pairs for validity, retaining valid matching point pairs that meet the preset matching conditions. The triangulation module is used to perform triangulation calculations on the matching point pairs based on the valid matching point pairs and the camera projection relationship, obtain spatial point information, introduce spatial point validity constraints, eliminate invalid spatial points, and form a valid set of three-dimensional spatial points. The pose estimation module is used to calculate the pose transformation matrix of the device at the current time relative to the previous time based on the effective set of three-dimensional spatial points at the current time and the correspondence between image features at adjacent time times. It is also used to update the absolute pose of the device at the current time in the world coordinate system based on the pose recursive model. The robust processing module is used to detect abnormal situations in the pose estimation process, trigger the robust processing mechanism, filter or process abnormal data, and also to perform pose solution quality assessment, pose estimation state discrimination and hierarchical management, and pose update strategy selection. The results output module is used to output the pose estimation results of the device in the world coordinate system at the current moment.

Images