A pose estimation method

By combining semantic segmentation and IMU pre-integration, a dynamic risk score is generated and mapped to continuous soft weights, which solves the robustness and accuracy problems of VIO system in dynamic scenarios and realizes stable pose estimation in highly dynamic environments.

CN123118779APending Publication Date: 2026-11-17HARBIN INSTITUTE OF TECHNOLOGY (SHENZHEN) (INSTITUTE OF SCIENCE AND TECHNOLOGY INNOVATION HARBIN INSTITUTE OF TECHNOLOGY SHENZHEN)
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202611643711.X
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2026-10-22
Publication Date
2026-11-17

AI Technical Summary

Technical Problem

Existing VIO systems suffer from poor robustness and low positioning accuracy in dynamic scenarios due to circular dependencies, semantic misjudgments, and lack of adaptive scheduling. In particular, in highly dynamic scenarios such as urban roads and park inspections, interference from dynamic objects severely affects positioning accuracy and robustness.

Method used

Semantic segmentation is used to generate a set of candidate dynamic feature points. Inertial motion prediction is performed by combining IMU pre-integration. Motion consistency residuals are calculated. The static confidence score of the trajectory and the visual quality score are fused to generate a dynamic risk score and map it into continuous soft weights. The visual reprojection residuals and IMU pre-integration residuals are then jointly optimized.

Benefits of technology

It effectively distinguishes between real dynamic features and static features, and adaptively adjusts and optimizes strategies to improve the accuracy and robustness of pose estimation in high dynamic scenarios, maintaining the continuity and accuracy of localization in complex environments.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN123118779A_ABST
    Figure CN123118779A_ABST
Patent Text Reader

Abstract

This invention discloses a pose estimation method, comprising: semantic segmentation of image frames to obtain pixel-level semantic information and generate a candidate dynamic feature point set; obtaining inertial motion prediction between adjacent image frames using IMU pre-integration and calculating motion consistency residuals; for feature point trajectories with historical data, calculating trajectory static confidence based on the historical sequence of motion consistency residuals and continuous tracking states; fusing pixel-level semantic information, motion consistency residuals, and trajectory static confidence to generate dynamic risk scores for candidate dynamic feature points and mapping them to feature-level continuous soft weights of visual reprojection residuals; calculating the visual quality score of the current image frame and mapping it to the frame-level overall weight of the visual reprojection residuals; and jointly optimizing the visual reprojection residuals and IMU pre-integration residuals to estimate the carrier pose. This invention can accurately distinguish between real dynamic features and static features and can adaptively adjust constraint weights when visual quality degrades.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] This invention relates to the interdisciplinary field of computer vision, robot navigation and localization, and in particular to a robust VIO (Visual-Inertial Odometry) pose estimation method applicable to high-dynamic scenarios such as urban roads, park inspections, and indoor and outdoor mobile robots. Background Technology

[0002] Traditional VIO (Visual-Inertial Odometry) systems are typically based on the static world assumption, which assumes that the spatial positions of landmarks in the environment are fixed and that their observed changes across different image frames are entirely caused by camera motion and sensor noise. Under this assumption, the system estimates the vehicle's pose, velocity, and bias by minimizing the visual reprojection residual and the IMU (Inertial Measurement Unit) pre-integration residual. However, when dynamic objects such as pedestrians and moving vehicles are present in the environment, the observed changes of feature points (landmarks) on these dynamic objects become mixed with the objects' own motion, causing interference or even failure in front-end matching, pose initialization, and back-end optimization, severely impacting the positioning accuracy and robustness of the VIO system in highly dynamic scenarios.

[0003] For the VIO problem in dynamic scenes, existing technologies mainly offer the following solutions: The first category is robust estimation methods based on geometric constraints, such as RANSAC or robust kernel functions. However, when the proportion of dynamic objects is high, the interior point set of these methods may be dominated by dynamic objects, leading to distortion of the baseline model itself and a circular dependency problem of "using contaminated visual results to verify visual features." The second category is dynamic region hard masking methods based on semantic segmentation, which directly delete all feature points located within preset dynamic category regions such as vehicles and pedestrians. However, semantic categories are not equivalent to physical motion states. For example, parked vehicles and pedestrians waiting at red lights are semantically "dynamic categories" but are actually stationary. Direct deletion leads to a sharp reduction in available visual constraints, especially in feature-sparse scenarios, causing system degradation. The third category introduces inertial information for auxiliary judgment, but existing solutions mostly remain at the level of simple thresholding and elimination, lacking a mechanism for unified confidence fusion of semantic priors, inertial motion constraints, and trajectory temporal information, and also lacking an adaptive scheduling strategy that dynamically adjusts the backend optimization weights based on the quality of visual observations.

[0004] Therefore, how to effectively distinguish between semantic "dynamic categories" and physical "real motion", how to retain effective features on static "pseudo-dynamic" targets, and how to adaptively adjust optimization strategies when visual observation quality degrades are technical problems that urgently need to be solved in this field.

[0005] The above background information is provided only to aid in understanding the concept and technical solution of this invention. It does not necessarily belong to the prior art of this patent application. In the absence of clear evidence that the above information was disclosed on the filing date of this patent application, the above background information should not be used to evaluate the novelty and inventiveness of this application. Summary of the Invention

[0006] To address the issues of poor robustness and low positioning accuracy in existing VIO systems in dynamic scenes due to circular dependencies, semantic misjudgments, and lack of adaptive scheduling, this invention proposes a pose estimation method that can accurately distinguish between real dynamic features and static features, and can adaptively adjust constraint weights when visual quality degrades.

[0007] To achieve the above objectives, the present invention adopts the following technical solution: In a first aspect, the present invention discloses a pose estimation method, comprising the following steps: S1: Perform semantic segmentation on the image frame to obtain pixel-level semantic information, and generate a set of candidate dynamic feature points based on a predefined set of dynamic categories; S2: Use IMU pre-integration to obtain inertial motion prediction between adjacent image frames, and calculate the motion consistency residual between the inertial predicted pixel coordinates and the actual observed pixel coordinates of candidate dynamic feature points; S3: For feature point trajectories with historical data, calculate the static confidence level of the trajectory based on the historical sequence of the motion consistency residual and the continuous tracking state; S4: Fuse the pixel-level semantic information, the motion consistency residual, and the trajectory static confidence to generate a dynamic risk score for the candidate dynamic feature points, and map the dynamic risk score to a feature-level continuous soft weight of the visual reprojection residual. S5: Calculate the visual quality score of the current image frame and map the visual quality score to the frame-level overall weight of the visual reprojection residual; S6: Combine the feature-level continuous soft weights and the frame-level overall weights to jointly optimize the visual reprojection residuals and IMU pre-integration residuals to estimate the carrier pose.

[0008] Preferably, step S1 specifically includes: Image frames are input into a semantic segmentation network to obtain pixel-level semantic category probability maps; Extract feature points from image frames and query the semantic category probability of the location of each feature point; Feature points whose maximum predicted probability belongs to a predefined dynamic category set and whose probability value exceeds a preset semantic filtering threshold are included in the candidate dynamic feature point set.

[0009] Preferably, the semantic segmentation network adopts a PIDNet structure, including proportional branches, integral branches, and differential branches, and outputs a pixel-level semantic category probability map through a boundary-guided fusion module.

[0010] Preferably, step S2 specifically includes: By using binocular parallax or depth information, feature points are back-projected from the image coordinate system to the camera coordinate system to obtain the three-dimensional coordinates of the feature points; The relative pose transformation between cameras in adjacent frames is derived using IMU pre-integration. The three-dimensional coordinates are projected onto the next frame image plane according to the relative pose transformation to obtain the pixel coordinates predicted by inertia; The Euclidean distance between the inertial prediction pixel coordinates and the actual pixel coordinates obtained from feature matching is calculated and used as the motion consistency residual.

[0011] Preferably, step S3 specifically includes: For each feature point trajectory, maintain a first-in-first-out historical residual queue to store motion consistency residuals from recent frames; Calculate the average value of the motion consistency residuals in the historical residual queue; The static confidence level of the trajectory is calculated based on the average value and the number of consecutive tracking frames of the trajectory.

[0012] Preferably, step S4 specifically includes: The motion consistency residual is normalized to a preset range; The normalized motion consistency residual, the semantic dynamic confidence obtained from pixel-level semantic information, and the trajectory static confidence are weighted and fused to obtain the dynamic risk score. The dynamic risk score is converted into a continuous soft weight between 0 and 1 using a monotonically decreasing mapping function.

[0013] Preferably, evaluating the visual quality of the current image frame in step S5 specifically includes: Calculate the ratio of the number of effectively tracked feature points in the current image frame to the number of reference points to obtain a quantity score; The image is divided into planar grids, and the entropy values ​​of feature points distributed in different grids are calculated to obtain the distribution uniformity score. Based on the semantic segmentation results, potential dynamic target regions are excluded, and the gray-level distribution difference between the current frame and the previous frame static image region is calculated to obtain the cross-frame photometric stability score. The image sharpness score is obtained by calculating the ratio of the variance of the Laplacian response of the image to the reference variance. The quantity score, distribution uniformity score, cross-frame photometric stability score, and image sharpness score are weighted or combined to evaluate the visual quality of the current image frame.

[0014] Preferably, the frame-level overall weight of the visual reprojection residual in step S5 is adjusted linearly or non-linearly between a preset low threshold and a high threshold based on the visual quality score.

[0015] Preferably, step S6 specifically includes: in sliding window optimization, using the feature-level continuous soft weights and the frame-level overall weights to weight the visual reprojection residuals, and combining the IMU pre-integration residuals and marginalized prior residuals to perform joint optimization to estimate the carrier pose.

[0016] In a second aspect, the present invention discloses a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the pose estimation method as described in the first aspect.

[0017] Compared with existing technologies, the beneficial effects of this invention are as follows: The pose estimation method provided by this invention generates a dynamic risk score by fusing semantic information, inertial information, and historical tracking information from multiple sources, and maps it to feature-level continuous soft weights. Simultaneously, it combines the frame-level overall weights obtained from the visual quality score mapping to jointly optimize the visual reprojection residual and the IMU pre-integration residual. This scheme first utilizes vision-independent IMU inertial motion prediction to verify the geometric consistency of candidate dynamic feature points generated based on semantic information, effectively eliminating interference from "false dynamic" semantic objects and identifying "true dynamic" features. Second, the introduction of static confidence in temporal trajectories allows features that have been tracked stably for a long time (such as parked vehicles) to obtain higher confidence, thus being effectively preserved and utilized, solving the problem of effective feature loss caused by traditional hard masking methods. Finally, the synergistic effect of feature-level continuous soft weights and frame-level overall visual quality weights not only achieves refined processing of the risk level of individual feature points but also enables the system to dynamically adjust the ratio of visual and inertial constraints according to the overall visual observation quality. This dual adaptive mechanism enables the optimization process to smoothly reduce the impact of unreliable visual information and strengthen the supporting role of inertial constraints in complex scenarios such as many dynamic objects, large changes in lighting, or blurred images, thereby significantly improving the accuracy, continuity, and robustness of pose estimation in highly dynamic and degraded scenarios.

[0018] In a further embodiment, the present invention also has the following beneficial effects: (1) By using a specific proportional-integral-differential network structure for semantic segmentation, local details, global context and edge information can be taken into account, thereby extracting dynamic category regions more accurately, providing more reliable semantic priors for subsequent processing, and indirectly improving the accuracy of dynamic feature recognition.

[0019] (2) By specifically defining the calculation method of motion consistency residual, namely using binocular depth and IMU pre-integration to predict and project three-dimensional coordinates, a quantitative and reliable geometric consistency metric is provided, which provides an accurate basis for subsequent confidence assessment and risk scoring.

[0020] (3) By defining the specific calculation method of trajectory static confidence, especially by introducing historical residual queue and continuous tracking maturity, it is possible to effectively distinguish between "unstable features that have just entered the field of vision", "stable features of long-term low residual tracking" and "features of abnormal motion trajectory", making the time-series dimension assessment more scientific and robust.

[0021] (4) By specifically defining the process of multi-source information fusion to generate continuous soft weights, the three heterogeneous information types of semantic, geometric and temporal information are unified into the same numerical range and weighted and fused, realizing the refined and smooth weight allocation of feature points of different risk levels, and avoiding the optimization problem caused by weight mutation.

[0022] (5) By comprehensively evaluating visual quality from four dimensions—feature quantity, spatial distribution, illumination stability, and sharpness—and dynamically mapping the overall weight of visual constraints accordingly, the system can make accurate and timely responses to various types of visual degradation scenarios, thereby maintaining robust pose estimation performance in challenging environments such as illumination changes, motion blur, and weak textures. Attached Figure Description

[0023] Figure 1 This is a flowchart of the overall process of the visual inertial odometry method of the present invention.

[0024] Figure 2 This is a flowchart of a pose estimation method according to a preferred embodiment of the present invention.

[0025] Figure 3 This is a schematic diagram of the PIDNet semantic segmentation network structure used in this invention.

[0026] Figure 4 This is a schematic diagram of semantic-geometric-temporal multi-source real-time confidence scoring and continuous soft weight generation.

[0027] Figure 5 This is a schematic diagram of the backend adaptive soft constraint scheduling for visual quality perception. Detailed Implementation

[0028] The present invention will now be described in further detail with reference to specific embodiments and the accompanying drawings. The same reference numerals denote the same parts unless otherwise specified. It should be emphasized that the following description is merely exemplary and not intended to limit the scope or application of the invention.

[0029] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments to provide a better understanding of the invention. However, the following embodiments do not limit the scope of the invention. Furthermore, it should be noted that the illustrations provided in the following embodiments are merely schematic representations of the basic concept of the invention. The drawings only show components relevant to the invention and are not drawn according to the actual number, shape, and size of the components in implementation. In actual implementation, the shape, quantity, and proportion of each component can be arbitrarily changed, and the component layout may also be more complex.

[0030] Based on research into existing methods for processing dynamic features, the following key issues have been identified in current technologies.

[0031] (1) The lack of motion verification in semantic constraints leads to the erroneous removal of valid features with false dynamics. Pure semantic methods equate dynamic categories with dynamic states, and vehicles, pedestrians, and cyclists are directly deleted as long as they fall into a preset category. However, in urban road and park scenarios, parked vehicles, pedestrians waiting at red lights, and briefly stopped moving targets often provide stable and well-distributed visual textures. If all of these are removed, the number of available constraints and spatial coverage in the backend will decrease, resulting in feature sparsity, weakened scale constraints, and degraded pose estimation. Based on this, the present invention addresses how to introduce a verification mechanism that can distinguish between "semantic dynamic categories" and "physical real motion" while preserving the advantages of semantic priors.

[0032] (2) Visual geometric constraints lack an independent benchmark, and dynamic discrimination suffers from circular dependencies. Geometric methods typically rely on visual matching points to estimate camera motion, and then use this estimation result to determine whether the matching points move in the same direction. When dynamic objects occupy a large proportion of the image or background features are insufficient, the camera motion estimation will be interfered with by dynamic points, causing the discrimination benchmark itself to be distorted, resulting in the error of retaining dynamic points and deleting static points. Based on this, the present invention solves the problem of how to use IMU pre-integration to form a short-term motion prior independent of the visual feature set, and use this prior to verify the geometric consistency of semantic candidate features.

[0033] (3) Gross error removal lacks temporal confidence and adaptive degradation scheduling, making it easy for dynamic errors to enter the prior. Existing methods mostly complete hard removal within a single frame or two adjacent frames, which cannot express intermediate states such as "just stopped moving", "reappearing after a short occlusion", or "long-term low residual stable tracking", nor can they actively reduce the global impact of visual observation when visual quality deteriorates. If keyframes containing dynamic errors are marginalized, the errors may be solidified into prior factors through Schur elimination and continue to propagate. Based on this, the present invention solves the problem of how to construct a continuous confidence score at the feature point level and feed back visual quality degradation information to the back-end constraint weights to form a stable adaptive soft constraint optimization mechanism.

[0034] To address the aforementioned issues, this invention proposes a pose estimation method, specifically a semantic-inertial dual verification and real-time confidence-constrained navigation method for visual-inertial odometry. This method utilizes semantic segmentation to provide candidate dynamic regions, IMU pre-integration to provide a visually decoupled motion reference, historical consistency residuals to provide temporal states, continuous soft weights to replace gross error removal, and dynamic scheduling of visual residuals and physical consistency constraints in the backend based on visual quality.

[0035] For ease of explanation, the following notation conventions are used consistently in this article: and Indicates adjacent image frames or adjacent state times; Indicates the number of consecutively tracked Visual feature trajectories; Represents a semantic category index; Indicates the first The feature point at the th ... Pixel coordinates in a frame image; This indicates that the feature point is at the th 3D coordinates in the camera coordinate system; superscripts W, B, and C represent the world coordinate system, IMU carrier coordinate system, and camera coordinate system, respectively. Additionally, This means transforming the homogeneous coordinates in coordinate system B to coordinate system A, i.e. Unless otherwise specified, all text is under subscript. All represent visual feature trajectory numbers, subscripts Both represent image frames or state moments.

[0036] The overall flow of the method of the present invention is as follows: Figure 1As shown, the system input consists of time-synchronized binocular images and IMU measurements, and the output is a six-DOF pose sequence. The method of this invention includes stages such as sensor data input, visual front-end processing, candidate dynamic feature verification and confidence modeling, visual quality evaluation, and back-end sliding window optimization. The first part consists of sensor data, including binocular image sequences and IMU measurement data; the binocular images are used for local feature extraction, cross-frame matching, binocular depth recovery, and semantic segmentation; the IMU measurement data is used to calculate the pre-integration between adjacent image frames. The system first generates candidate dynamic features based on semantic probabilities, then uses inertial prediction motion for consistency verification, and combines historical residuals and continuous tracking states to generate feature-level soft weights; the back-end adaptively weights the visual reprojection residuals based on the feature-level soft weights and frame-level visual quality scores, and together with the IMU pre-integration residuals and marginalization priors, completes state estimation. Specifically, the second part is the visual odometry front-end. It obtains candidate dynamic features through the following steps: image distortion correction / time synchronization → feature extraction and cross-frame matching (superpoint + lightglue) → binocular parallax recovery of feature point 3D coordinates → PIDNet semantic segmentation with dynamic class probability / masking → candidate dynamic feature selection. It then uses IMU pre-integration of relative motion benchmarks between adjacent frames, performs semantic-inertial joint verification based on inertial-visual motion consistency residuals, and performs trajectory temporal modeling based on historical residuals and the number of consecutive tracking frames. Real-time confidence scoring is then performed based on semantic, geometric, and temporal 3D scoring to obtain the front-end output, including feature observations, keyframes, and soft weights. The third part is the back-end nonlinear optimization. It first performs sliding window optimization based on visual reprojection residuals, IMU pre-integration constraints, translation-velocity consistency constraints, continuous soft weights, and visual quality adaptive scheduling; then it evaluates visual quality based on the number of feature points, distribution uniformity, illumination stability, and image sharpness. The fourth part is the mapping module, which includes keyframe management → map point maintenance → local dense map updates. The final system output includes six-DOF pose, velocity, IMU bias, and local map.

[0037] The key aspects of the method in this invention include: preprocessing of binocular images and IMU data, local feature extraction and cross-frame matching, construction of candidate dynamic features based on semantic segmentation, inertial-visual consistency verification using IMU pre-integration as a short-term motion benchmark, trajectory static confidence modeling based on historical consistency residuals, real-time risk scoring and continuous soft weight generation from semantic-geometric-temporal multi-source sources, and back-end adaptive weighted optimization for visual quality perception.

[0038] like Figure 2 As shown, the pose estimation method disclosed in the preferred embodiment of the present invention includes the following steps: S1: Perform semantic segmentation on the image frame to obtain pixel-level semantic information, and generate a set of candidate dynamic feature points based on a predefined set of dynamic categories; Specifically, step S1 includes: S11: Acquire image frames through sensor synchronization and preprocessing; The system employs a binocular camera and an IMU as the sensor suite, and spatial and temporal calibrations were pre-commissioned. First, the focal length, principal point, and distortion coefficients of the left and right cameras were calibrated, along with the relative rotation matrix and translation vector between the left and right cameras. Then, the extrinsic parameter transformation between the camera coordinate system and the IMU carrier coordinate system was calibrated. and the fixed time offset between the camera timestamp and the IMU timestamp .

[0039] The Kalibr multi-sensor joint calibration tool is employed to jointly estimate the intrinsic and extrinsic parameters of the stereo camera, the spatial extrinsic parameters of the camera-IMU, and the temporal offset of the camera-IMU using a calibration sequence that includes stereo images and IMU measurements. When hardware synchronization is available, the stereo camera and IMU share a clock or trigger signal; when hardware synchronization is not available, the IMU measurements are interpolated based on the timestamps of each sensor and the time offset obtained from calibration, and then aligned to the corresponding image exposure time.

[0040] After calibration, distortion correction and epipolar correction are performed on the binocular images. Initial bias estimation and necessary noise filtering are performed on the IMU angular velocity and linear acceleration. The IMU measurements between the exposure times of adjacent image frames are divided into corresponding pre-integration intervals, thereby providing a unified time and geometric reference for feature matching, binocular depth recovery, inertial pre-integration and coordinate transformation.

[0041] S12: Input the image frame into the semantic segmentation network to obtain a pixel-level semantic category probability map; This invention preferably employs the PIDNet real-time semantic segmentation network to generate pixel-level semantic category probability maps. PIDNet is a three-branch semantic segmentation network inspired by a proportional-integral-derivative (PI-D) controller structure, comprising a proportional branch, an integral branch, and a derivative branch. The proportional branch preserves high-resolution local details in the image, the integral branch aggregates large-scale scene context information, and the derivative branch extracts target edges and semantic region boundary information. These three types of features are fused by a boundary-guided fusion module to obtain pixel-level semantic segmentation results that balance region integrity and boundary accuracy. Visual inertial odometry requires continuous, real-time processing of image data, and candidate dynamic features are typically located at the edges and local texture regions of targets such as vehicles and pedestrians. Therefore, using PIDNet, which balances inference speed, contextual representation, and boundary response capabilities, is beneficial for improving the real-time performance of dynamic category region extraction and the stability of edge localization.

[0042] refer to Figure 3The input image first passes through the backbone feature extraction network, and then enters the proportional branch, integral branch, and differential branch respectively. The proportional branch preserves high-resolution details, the integral branch extracts a wide range of contextual information, and the differential branch highlights the target boundary. After boundary-guided fusion, the features from the three branches output a pixel-by-pixel semantic category probability map. This invention extracts the probabilities of potential dynamic categories such as vehicles, pedestrians, and cyclists from this probability map to construct a candidate dynamic feature set.

[0043] For the Frame left eye image A self-supervised feature point detection and description network (SuperPoint) is employed. SuperPoint is a fully convolutional local feature extraction network that can simultaneously output the feature point location probability and the corresponding local feature descriptor in a single forward computation, thereby obtaining the first... Frame feature point set and descriptor subsets ,in Indicates the first The number of feature points detected in a frame. Compared to manually designed corner detectors and descriptors, SuperPoint can jointly learn feature detection and descriptive representations through a data-driven approach, which is beneficial for improving the ability to detect feature repetition in areas with changes in illumination, viewpoint, and weak texture.

[0044] The first Frame and the The feature point locations and descriptors of each frame are input into a lightweight local feature matching network (LightGlue) based on an attention mechanism. LightGlue uses self-attention and cross-attention mechanisms to exchange contextual information between local features of two frames, calculates matching probabilities based on feature descriptors, feature locations, and their interrelationships, and obtains a cross-frame matching set. And the corresponding matching confidence level.

[0045] Traditional sparse optical flow typically relies on the assumptions of local grayscale consistency and small displacements between adjacent frames to iteratively search for feature locations within a local neighborhood. LightGlue, on the other hand, directly establishes a correspondence based on the local feature descriptions of two frames and the global context, without requiring feature points to strictly satisfy the conditions of constant local brightness and small displacements. Therefore, under conditions of large pixel displacements, local occlusion, viewpoint changes, or weak textures, LightGlue can provide a more stable matching relationship for cross-frame trajectory construction.

[0046] Simultaneously, the first Frame Image Input the semantic segmentation network to obtain pixel-level semantic category probability maps. : (1) In the formula, Represents the pixel coordinates in the image. Represents a semantic category index. Indicates that in the given first Frame Image Under the condition of pixels Belongs to semantic category The predicted probability.

[0047] S13: Extract feature points from the image frame and query the semantic category probability of the location of the feature points; S14: Feature points whose maximum predicted probability belongs to the predefined dynamic category set and whose probability value exceeds the preset semantic filtering threshold are included in the candidate dynamic feature point set.

[0048] Let the predefined set of potential dynamic categories be Categories include, for example, vehicles, pedestrians, cyclists, motorcycles, and bicycles. The matching feature trajectory in the th Semantic dynamic confidence of frames Defined as: (2) In the formula, This represents a predefined set of potential dynamic categories.

[0049] When the maximum predicted class of the feature point belongs to the potential dynamic class set, and Not less than the semantic candidate selection threshold When this happens, add it to the candidate dynamic feature set. : (3) In the formula, The threshold for semantic candidate selection. Indicates the first A set of candidate dynamic feature trajectory indices for a frame. The value can be determined based on the recall rate, false positive rate, or comprehensive recognition index of the dynamic categories in the validation dataset. In experiments, 0.5 can be used as the initial value, and then adjusted according to the missed detection and false positive rates in the actual scenario. Features that do not enter the candidate dynamic feature set are considered low-risk features and enter subsequent optimization with default weights; features that enter the candidate set are not directly deleted, but continue to undergo inertial-visual consistency verification and temporal confidence evaluation.

[0050] This step filters candidate dynamic features by combining semantic category probability maps with preset thresholds. This approach retains the advantages of semantic priors while avoiding misjudgments of low-probability semantic regions, making the generation of candidate sets more accurate.

[0051] S2: Use IMU pre-integration to obtain inertial motion prediction between adjacent image frames, and calculate the motion consistency residual between the inertial predicted pixel coordinates and the actual observed pixel coordinates of candidate dynamic feature points; This step uses the short-term motion prior provided by IMU pre-integration as an independent geometric verification benchmark to resolve the cyclic dependency problem in estimating camera motion from visual matching points. Specifically, step S2 includes: S21: Derive the relative pose transformation between cameras in adjacent frames using IMU pre-integration; In the Frame and the Between frames, the IMU angular velocity and linear acceleration measurements within the corresponding time interval are pre-integrated to obtain the rotational pre-integrated quantity. Velocity pre-integral quantity and displacement pre-integral quantity To avoid re-executing the entire pre-integration after each IMU bias update, a first-order correction is performed using the Jacobian matrix of the pre-integrated quantity with respect to the gyroscope and accelerometer biases: (4) (5) (6) In the formula, and These represent the linearization points of the accelerometer bias and the gyroscope bias, respectively. and This represents the change in bias relative to the linearization point; This represents the Jacobian matrix of the corresponding preintegral with respect to the bias. Represents the rotational pre-integral quantity Gyroscope bias Jacobian matrix, Indicates the pre-integral quantity of velocity Accelerometer bias Jacobian matrix, Indicates the pre-integral quantity of velocity Gyroscope bias Jacobian matrix, Indicates the pre-integral quantity of displacement Accelerometer bias Jacobian matrix, Indicates the pre-integral quantity of displacement Gyroscope bias The Jacobian matrix; Exp(·) denotes the exponential mapping from the rotation vector to the rotation matrix. When the backend optimizes and updates the IMU bias, the above first-order correction can be used to quickly update the pre-integral.

[0052] S22: Using binocular parallax or depth information, feature points are back-projected from the image coordinate system to the camera coordinate system to obtain the three-dimensional coordinates of the feature points; For candidate dynamic feature set The first in The characteristic trajectory, using its first characteristic trajectory, is used in the second characteristic trajectory. Disparity recovery depth in left and right eye images And through the camera back projection function π -1 (·) Obtain the feature point at the th 3D coordinates in the frame camera coordinate system : (7) S23: Project the three-dimensional coordinates onto the next frame image plane according to the relative pose transformation to obtain the pixel coordinates predicted by inertia; According to the Frame to the The relative motion prediction of the carrier is constructed by using the IMU pre-integral values ​​between frames, the velocity and bias estimates of the previous time step, and the camera-IMU extrinsic parameters. And convert it into a relative transformation between adjacent camera frames: (8) In the formula, This represents the camera coordinate system of the k-th frame, predicted based on IMU pre-integration. To the Frame camera coordinate system The relative pose transformation matrix, This represents the extrinsic transformation matrix from the IMU carrier coordinate system B to the camera coordinate system C. This represents the IMU carrier coordinate system at time k, obtained from IMU pre-integration. IMU carrier coordinate system at time k+1 The relative pose transformation matrix, This represents the extrinsic transformation matrix from the camera coordinate system C to the IMU carrier coordinate system B.

[0053] The three-dimensional feature points are projected onto the first according to the inertial prediction relative motion. From the frame image plane, the inertial prediction pixel coordinates are obtained. : (9) In the formula, and These are the rotation matrix and translation vector in the relative transformation of inertial prediction, respectively. This represents the projection function from a 3D point in the camera coordinate system onto the image plane.

[0054] S24: Calculate the Euclidean distance between the pixel coordinates predicted by inertia and the actual pixel coordinates obtained by feature matching, as the motion consistency residual.

[0055] The Euclidean distance between the inertial predicted pixel and the actual matched observed pixel is defined as the inertial-visual motion consistency residual. for: (10) In the formula, the subscript Indicates the visual feature trajectory number. and This indicates adjacent image frames. In this paper, the arriving frame corresponding to the residual is used as the residual index; therefore, Indicates by the first Frame to the The motion consistency residual formed by frame inertial prediction and actual visual observation, correspondingly, Indicates by the first Frame to the Motion consistency residuals formed by frames; This represents the actual observed pixel coordinates of the i-th feature point in the (k+1)-th frame. The IMU measurement and pre-integration process used in the above prediction do not depend on the current set of candidate visual features to be verified, thus reducing the circular dependency caused by estimating camera motion using the same set of visual features and then verifying these features again. Features on real dynamic objects contain the object's own motion, and their actual observed positions usually deviate significantly from the inertial predicted positions; features on physically stationary targets such as parked vehicles mainly follow the camera's motion, and their consistency residuals are usually smaller.

[0056] This step, by employing the PIDNet network structure, can simultaneously preserve details in semantic segmentation, aggregate context, and extract boundaries, ensuring accurate identification of dynamic regions and precise localization of candidate feature points. Furthermore, by utilizing binocular parallax or depth information to recover the 3D coordinates of feature points and performing inertial predictive projection, this step constructs an objective geometric verification benchmark that is completely decoupled from visual matching. This fundamentally solves the circular dependency problem of "verifying visual features with contaminated visual results" in existing technologies.

[0057] S3: For feature point trajectories with historical data, calculate the static confidence level of the trajectory based on the historical sequence of the motion consistency residual and the continuous tracking state; This step extends the dynamic state evaluation of feature points from a single frame to a time series dimension by using a motion state confidence modeling method based on semantic-inertial residual consistency verification, thereby capturing their long-term motion properties.

[0058] Specifically, step S3 includes: S31: Maintain a first-in-first-out historical residual queue for each feature point trajectory to store motion consistency residuals from recent frames; For each feature trajectory that continuously appears in cross-frame matching, maintain a maximum length of First-in-first-out historical residual queue, index This represents the history window, used to store the inertial-visual consistency residuals in the most recent consecutive frames and record the number of uninterrupted continuous tracking frames for that trajectory. Queue length Based on the preset historical time window and camera frame rate Sure: (11) In the formula, and These are the minimum and maximum allowed queue lengths, respectively. First, based on... Calculate the number of image frames corresponding to the time window, round it to the nearest integer, and then limit it to a certain value. Within the range, This represents the rounding function. Using a time window instead of a fixed number of frames allows for adaptive adjustment of the queue size as the image frame rate changes, ensuring a relatively stable historical time span. In the experiment, A time interval of 0.5 to 1.0 seconds is acceptable; when the trajectory history length is insufficient... When using all currently valid residuals.

[0059] S32: Calculate the average value of the motion consistency residuals in the historical residual queue; set up Indicates the number of currently available historical residuals, then the historical average consistency residual of this trajectory. for: (12) when At that time, the historical average residual is not calculated, and the static confidence level of the trajectory is set to 0.

[0060] S33: Calculate the static confidence level of the trajectory based on the average value and the number of consecutive tracking frames of the trajectory.

[0061] Based on continuous tracking of maturity and historical residuals, the first... The trajectory in the first static confidence of frame trajectory : (13) In the formula, The number of continuous tracking frames required for the trajectory to reach a mature state can be determined based on a preset stabilization time. With the camera frame rate determined, i.e. ; This is the Gaussian decay scale for historical residuals, used to adjust the degree of influence of residuals on static confidence. The time can be set according to the minimum stable observation time expected by the system. The preferred time is 0.3 to 1.0 seconds.

[0062] It can be determined based on the statistical distribution of consistency residuals in the static calibration sequence. For example, let the inertial-visual consistency residual reference threshold be... And expect when the historical average residual equals If the Gaussian decay term decreases to 0.5, then it can be taken as... Alternatively, it can be determined based on the standard deviation, quantiles, or median absolute deviation of the static sample residuals. When the characteristic trajectory is stably tracked for a long time with low residuals, Approaching 1; when the trajectory has just come into view or the historical residuals are consistently large, Approaching 0.

[0063] This step extends the instantaneous judgment of a single frame to a continuous evaluation in the time dimension by maintaining a historical residual queue and calculating the static confidence of the trajectory. It can effectively capture intermediate states such as "just stopped moving" or "long-term stillness", making the state evaluation of feature points more stable and reliable.

[0064] S4: Fuse the pixel-level semantic information, the motion consistency residual, and the trajectory static confidence to generate a dynamic risk score for the candidate dynamic feature points, and map the dynamic risk score to a feature-level continuous soft weight of the visual reprojection residual. This step integrates three types of information—semantic prior, geometric residual, and temporal confidence—into a unified continuous risk score, generating soft weights to replace traditional gross error removal.

[0065] Specifically, step S4 includes: S41: Normalize the motion consistency residual to a preset range; To ensure that the inertial-visual consistency residual, semantic probability, and trajectory static confidence are within the same numerical range, the residual is normalized as follows: (14) In the formula, This represents the normalized inertial-visual motion consistency residual corresponding to the i-th visual feature trajectory in the k-th frame. This serves as a reference threshold for inertial-visual consistency residuals, which can be determined based on the residual statistical distribution in a static scene, camera pixel noise levels, or a validation set. When the residual exceeds... It is truncated to 1 to avoid extreme outliers having an excessive impact on the risk score.

[0066] S42: The normalized motion consistency residual, the semantic dynamic confidence obtained from pixel-level semantic information, and the trajectory static confidence are weighted and fused to obtain the dynamic risk score; A dynamic risk score is obtained by integrating semantic dynamic confidence, normalized consistency residuals, and trajectory static confidence. : (15) (16) In the formula, , and These represent the fusion weights for semantic, instantaneous geometric consistency, and trajectory temporal information, respectively, and can be determined based on the validation set positioning error, dynamic point recognition accuracy, or ablation experiment results.

[0067] S43: The dynamic risk score is converted into a feature-level continuous soft weight between 0 and 1 by a monotonically decreasing mapping function.

[0068] The dynamic risk score is mapped to feature-level continuous soft weights of backend visual observations using a monotonically decaying function. : (17) In the formula, This is the soft-weight decay coefficient. When... When approaching 0, Approaching 1; when dynamic risk increases, The confidence level is continuously reduced to avoid directly deleting intermediate confidence features.

[0069] refer to Figure 4 This diagram illustrates the real-time confidence score generation and continuous soft weight generation of semantic-geometric-temporal multi-source systems. The semantic dynamic confidence score, normalized inertial-visual consistency residual, and trajectory static confidence score of feature points represent semantic category risk, instantaneous motion anomaly degree, and historical trajectory stability, respectively. These three types of information are normalized and weighted to generate a dynamic risk score, which is then used to generate continuous soft weights between 0 and 1 through a monotonically decaying mapping function. These soft weights are applied to the visual reprojection residuals of the corresponding feature points, allowing features with different confidence levels to participate in backend optimization with different weights.

[0070] This step normalizes and weights multi-source information and maps it to continuous soft weights via a monotonically decreasing function, thereby achieving a fine quantification of the risk level of feature points. This ensures that the contribution of features to the optimization process is smoothly transitioned, avoiding abrupt changes in the information matrix and optimization oscillations caused by hard threshold removal.

[0071] S5: Calculate the visual quality score of the current image frame and map the visual quality score to the frame-level overall weight of the visual reprojection residual; The backend uses a sliding window optimization to model the visual reprojection residual, the full IMU pre-integration residual, and the marginalized prior residual in a unified manner.

[0072] Specifically, step S5 includes: S51: Calculate the visual quality score of the current image frame, i.e., construct a four-dimensional visual quality score. ; This step S51 specifically includes: S511: Calculate the ratio of the number of effective tracked feature points in the current image frame to the number of reference points to obtain a quantity score; Let the number of effective tracked feature points in the current frame be... The number of reference features required for normal state estimation is The score for the number of effective features is then calculated. for: (18) In the formula, This is a truncation function used to restrict input values ​​to a given range.

[0073] S512: The image is divided into planar grids, and the entropy values ​​of feature points in different grid distributions are calculated to obtain the distribution uniformity score; Divide the image plane into Let the k-th frame be the grid of the k-th image. The number of valid features within each grid is And define the proportion of the number of valid visual feature points in this grid to the total number of valid visual feature points in the current frame as . Then, the uniformity of the feature distribution is calculated using the normalized spatial entropy. : (19) In the formula, , To prevent extremely small positive numbers from resulting in zero values ​​in logarithmic calculations, the more uniformly the features are distributed across the grid, the better. The closer to 1, the more concentrated the features are in a few regions. The smaller the value, the better. g represents the current grid number being considered, and m represents the index used to traverse all grids during the summation process. This represents the number of valid visual feature points within the g-th grid region in the k-th frame image. This represents the number of valid visual feature points in the m-th network region during the summation process.

[0074] S513: Based on the semantic segmentation results, potential dynamic target regions are excluded, and the gray-level distribution difference between the current frame and the previous frame static image region is calculated to obtain the cross-frame photometric stability score. To reduce the interference of scene content changes caused by dynamic targets such as vehicles and pedestrians entering and leaving the image area on the photometric stability evaluation, based on the semantic segmentation results obtained in step S1, the image areas corresponding to potential dynamic categories such as vehicles, pedestrians, and cyclists are excluded from the photometric statistics area, and only the remaining static candidate areas are used to calculate the grayscale histogram.

[0075] Let the normalized grayscale histograms of the current frame and the previous frame after excluding potential dynamic category regions be respectively... and The difference in luminance between static regions of two adjacent frames is defined as follows: (20) And calculate the cross-frame photometric stability score based on the photometric differences: (twenty one) In the formula, and These represent the normalized grayscale histograms obtained in the k-th and (k-1)-th frames after excluding potential dynamic category regions, respectively. Frequency within a grayscale range; Indicates the number of intervals in the grayscale histogram; Indicates the range index of the grayscale histogram; This indicates the difference in grayscale distribution between two adjacent static candidate regions; This represents the cross-frame photometric stability score for the k-th frame; This represents the photometric difference scale parameter, used to adjust the sensitivity of the photometric stability score to changes in grayscale distribution.

[0076] By excluding potential dynamic category regions before grayscale histogram statistics, the interference of scene content changes caused by large-area dynamic targets such as vehicles and pedestrians entering or leaving the field of view on photometric stability evaluation can be reduced. When the grayscale distribution of static candidate regions changes little between adjacent frames, Approaching 1; when a significant overall luminosity change occurs in the static candidate region, The corresponding reduction.

[0077] S514: Calculate the ratio of the variance of the Laplacian response of the image to the reference variance to obtain the image sharpness score; Image sharpness is evaluated using the variance of the Laplacian response of grayscale images: (twenty two) In the formula, The reference Laplace variance for sharp images can be obtained statistically from sharp images in the calibration sequence or validation set. The sharper the image, the better. The closer it is to 1. This represents the variance of the Laplacian response of the k-th grayscale image. This represents the variance operation, used to calculate the degree of dispersion of input data relative to its mean. Represents the gradient operator, This represents the grayscale image of the k-th frame. This represents the sharpness score of the k-th frame image.

[0078] S515: Weight or combine the quantity score, distribution uniformity score, cross-frame photometric stability score, and image sharpness score to evaluate the visual quality of the current image frame.

[0079] The visual quality score of the current frame is obtained by combining the above four indicators. : (twenty three) (twenty four) In the formula, This indicates the weight of each dimension.

[0080] This step comprehensively evaluates visual quality from four dimensions: quantity, distribution, illumination, and sharpness. It can fully and objectively reflect the reliability of the visual information in the current frame, providing an accurate basis for subsequent adaptive scheduling.

[0081] S52: Map the visual quality score to the frame-level global weight of the visual reprojection residual; Among them, the frame-level overall weight of the visual reprojection residual is adjusted linearly or non-linearly between a preset low threshold and a high threshold according to the visual quality score.

[0082] Weights of each dimension These values ​​can be initially set to the same value, or adjusted based on the correlation between each quality metric and the validation set localization error. The visual quality score is mapped to the overall weight of the visual residual. (i.e., frame-level overall weight): (25) In the formula, and These represent the low and high thresholds for visual quality, respectively. and These are the lower and upper limits of the overall weight of the visual residual, respectively. When When the height is high, make full use of visual observation; when As the visual residual decreases, the overall weight of the visual residual is reduced.

[0083] This step maps visual quality scores by setting high and low thresholds, achieving smooth adjustment of the overall visual weight, suppressing sudden changes in weight at critical states, and improving the stability of the system.

[0084] S6: Combine the feature-level continuous soft weights and the frame-level overall weights to jointly optimize the visual reprojection residuals and IMU pre-integration residuals to estimate the carrier pose.

[0085] Step S6 specifically includes: In the sliding window optimization, the visual reprojection residual is weighted using the feature-level continuous soft weights and the frame-level overall weights, and combined with the IMU pre-integration residuals and marginalized prior residuals to perform joint optimization to estimate the carrier pose. Specifically, within the sliding window optimization framework, the feature-level soft weights and frame-level overall weights are uniformly applied to the visual residuals, achieving a comprehensive adaptive constraint that combines point and surface approaches, maximizing the robustness and accuracy of the joint optimization.

[0086] To avoid redundant weighting of the same IMU measurements, this invention no longer repeatedly adds translation and velocity residuals as additional constraints independent of IMU pre-integration, but instead treats them as components of the complete IMU pre-integration residuals. (Back-end total cost function) Revised to: (26) In the formula, This represents the set of state variables to be optimized within the sliding window, including the carrier pose, velocity, accelerometer bias, gyroscope bias, and visual landmark parameters at each state time. For marginalized priors; Indicates the first The set of effective visual observations used for frame optimization; For the first Visual reprojection residuals corresponding to visual feature trajectories; Continuous weights at the feature level; As the overall weight for frame-level visual quality; For robust kernel functions; For the first Frame to the Full IMU pre-integrated residuals between frames; and Let represent the covariance matrices of the corresponding visual observations and IMU pre-integration factors, respectively. The weighted square norm is defined as: (27) In the formula, Represents the residual vector In the covariance matrix The corresponding information matrix Weighted square norm Represents the residual vector. This represents the transpose of the residual vector. Represents the covariance matrix The inverse matrix.

[0087] The complete IMU pre-integration residuals include position, rotation, velocity, and bias random walk residuals. Among these, the position residuals... Velocity residual Rotational residuals and biased random walk residuals Write them as follows: (28) (29) (30) (31) In the formula, and The first The position and velocity of the IMU carrier in the world coordinate system at each frame time; This is the rotation matrix from the IMU carrier coordinate system to the world coordinate system; This is the gravity vector in the world coordinate system; The interval between adjacent states; and These are the displacement pre-integral and velocity pre-integral after first-order offset correction, respectively. This illustrates the logarithmic mapping from the rotation matrix to the rotation vector; This represents the rotational pre-integral quantity after first-order correction for gyroscope bias. Constrain the random walk variations of accelerometer bias and gyroscope bias between adjacent states respectively. The bias vector representing the acceleration at time k is given. This represents the gyroscope bias vector at the k-th state. The position, rotation, velocity, and bias random walk residuals together constitute the complete IMU pre-integration residual. : (32) Among them, the position, velocity, and rotation residuals are all 3-dimensional, and the random walk residuals of the accelerometer bias and gyroscope bias are each 3-dimensional. Therefore, the complete IMU pre-integration residuals are... It is a 15-dimensional vector.

[0088] refer to Figure 5This diagram illustrates the backend adaptive soft-constraint scheduling for visual quality perception. The system calculates a visual quality score based on the number of effective feature points, feature space distribution, illumination stability, and image sharpness, and establishes low, medium, and high visual states using low-quality and high-quality thresholds. When visual quality is high, the overall weight of the visual residual is increased to fully utilize visual information; when visual quality is medium, a balance is struck between information utilization and robustness; when visual quality is low, the overall weight of the visual afterimage is reduced, allowing the IMU pre-integration constraint to occupy a relatively higher proportion in the total cost function.

[0089] The pose estimation method proposed in the preferred embodiment of this invention first utilizes semantic segmentation to generate a set of candidate dynamic feature points, rather than directly deleting them, thus preserving potentially usable static features. Next, it uses vision-independent motion prediction obtained through IMU pre-integration to verify the motion consistency of the candidate feature points, breaking the circular dependency of "verifying visual features with visual estimation" in traditional methods, and enabling more accurate identification of feature points in true motion. Based on this, by analyzing the historical sequence of motion consistency residuals and continuous tracking states, a temporal dimension of static confidence is assigned to the feature point trajectories, allowing "pseudo-dynamic" features with long-term stable tracking (such as points on parked vehicles) to obtain high confidence. Finally, a dynamic risk score is generated by fusing semantic, geometric consistency, and temporal confidence, and mapped to continuous soft weights, replacing the traditional binary processing of hard "removal" or "retention." This allows feature points with different confidence levels to participate in optimization with different weights, avoiding optimization instability caused by information mutations. Simultaneously, by introducing frame-level visual quality scoring and dynamically adjusting the overall weight of visual constraints, the supporting role of inertial constraints is automatically enhanced when visual degradation occurs. The above mechanisms work together to form a multi-level robust optimization framework that ranges from instantaneous single-point judgment to long-term trajectory evaluation and overall visual quality scheduling, significantly improving the accuracy and robustness of pose estimation in dynamic scenes.

[0090] Specifically, the present invention proposes the following: (1) A dynamic candidate verification algorithm based on PIDNet semantic initial screening and IMU pre-integration residual. This algorithm uses the dynamic category probability output by PIDNet to generate the candidate set, instead of directly using it as the basis for deletion; then, it uses binocular depth, camera-IMU extrinsic parameters and IMU pre-integration to obtain the pure inertial predicted pixel coordinates, and constructs the inertial-visual motion consistency residual. Among them, IMU pre-integration is used as a verification benchmark independent of visual motion estimation to perform secondary geometric verification on the features in the semantic dynamic category, thereby distinguishing between true dynamic and false dynamic targets.

[0091] (2) A trajectory static confidence update algorithm based on historical consistency residuals. This method maintains a historical inertial-visual consistency residual queue for continuously tracked visual feature trajectories and records the number of uninterrupted continuous tracking frames of the trajectory. ; Calculate the historical average consistency residual based on the number of valid historical residuals of the trajectory. By combining the trajectory continuous tracking maturity and the Gaussian decay term of historical residuals, the first... The characteristic trajectory in the first static confidence of frame trajectory Specifically, the instantaneous dynamic judgment based on a single frame or adjacent frames is extended to a trajectory-level static confidence level that can be continuously updated over time. This allows features such as stationary vehicles that maintain low consistency residuals and are stably tracked over a long period to gradually increase their trajectory static confidence level and obtain higher weights in subsequent risk scoring and soft weight mapping. For features that have just entered the field of view, have a short continuous tracking time, or have unstable historical residuals, a lower static confidence level is maintained to reduce the risk of their weights being incorrectly increased.

[0092] (3) An adaptive threshold scheduling algorithm based on multi-source risk scoring and visual quality. This method integrates semantic dynamic confidence. Normalized inertia-visual consistency residual static confidence of trajectory A feature-level dynamic risk score is obtained. The dynamic risk score is mapped to continuous soft weights of visual observations using a monotonically decaying function. Simultaneously, a visual quality score is calculated based on the number of effective tracking features in the current frame, the uniformity of feature spatial distribution, the illumination stability of adjacent frames, and image sharpness. The visual quality score is then mapped to the overall weight of the frame-level visual residual. Specifically, feature-level continuous soft weights are used to replace the hard deletion of candidate dynamic features, allowing visual features with different risk levels to participate in backend optimization with different weights. At the same time, the visual quality evaluation results are fed back to the sliding window optimization. By dynamically adjusting the overall weight of the visual reprojection residuals, the relative role of the full IMU pre-integration constraint in the total cost function is enhanced when visual quality deteriorates. This forms a visual inertial robust state estimation mechanism that coordinates feature-level credibility evaluation and frame-level visual constraint scheduling.

[0093] Compared with existing technical solutions, the present invention has the following advantages: (1) This invention can reduce visual circular dependence in the dynamic feature identification process. The consistency verification benchmark of candidate features comes from the IMU measurement and its pre-integration results between adjacent image frames, rather than being obtained by refitting the candidate visual features to be verified. Therefore, even if dynamic targets such as vehicles and pedestrians occupy a large proportion in the image, their motion is not likely to cause the overall verification benchmark to be biased towards dynamic targets by jointly participating in the visual model fitting, which is beneficial to improving the stability of dynamic feature judgment in scenarios with a high proportion of dynamic targets.

[0094] (2) This invention can retain effective features on pseudo-dynamic targets such as parked vehicles while suppressing real dynamic features. The semantic segmentation results are only used to generate candidate sets and are not directly used as the basis for deletion. For objects such as parked vehicles, although their semantic dynamic confidence is relatively high, their trajectory static confidence gradually increases due to the small residual between the inertial prediction pixels and the actual observed pixels, and the ability to track continuously and stably for a long time. This reduces the final dynamic risk score and obtains a higher soft weight. Therefore, this type of feature can still provide effective geometric constraints to the backend and reduce the loss of feature information caused by semantic hard masking.

[0095] (3) This invention can reduce optimization degradation caused by sudden changes in the number of effective constraints during dynamic feature processing. Traditional hard thresholding methods will cause the weights of features to change directly from normal values ​​to zero when the feature state crosses the threshold, which may cause sudden changes in the number and spatial distribution of visual constraints within the sliding window. This invention integrates semantic, inertial geometry and trajectory temporal information into a continuous risk score, and maps it to soft weights through a continuous function, so that the contribution of features to the objective function and information matrix changes smoothly with the degree of risk, thereby helping to improve the optimization continuity and numerical stability in weak texture or feature sparse scenes.

[0096] (4) This invention can reduce the continuous contamination of marginalization priors by low-quality visual observations. When the number of effective features decreases, the feature distribution becomes concentrated, the illumination changes increase, or the image becomes blurred, the visual quality score decreases, and the system correspondingly reduces the overall weight of the visual reprojection residual. Therefore, before the sliding window performs state optimization and marginalization, the influence of unreliable visual observations on the state increment and information matrix has been weakened, thereby reducing the probability that keyframes containing dynamic errors or image degradation errors are written into the long-term priors through the marginalization process, and improving the robustness during continuous operation.

[0097] Another preferred embodiment of the present invention discloses a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the pose estimation method as described in Embodiment 1 above.

[0098] Optionally, the aforementioned storage media may include, but are not limited to, USB flash drives, read-only memory (ROM), random access memory (RAM), portable hard drives, magnetic disks, or optical disks, and other media capable of storing computer programs.

[0099] To address the problems of insufficient accuracy in dynamic feature recognition, limited back-end constraint compensation capability when visual quality degrades, and susceptibility to dynamic object interference in pose estimation of traditional visual inertial odometry systems in dynamic scenes, this invention proposes a semantic-inertial dual verification and real-time confidence-constrained navigation method for visual inertial odometry, utilizing existing binocular camera and inertial measurement unit (IMU) data without adding additional sensors such as lidar or millimeter-wave radar. This method first obtains semantic prior information of potential dynamic categories such as vehicles and pedestrians through semantic segmentation, and marks visual feature points located in relevant semantic regions as candidate dynamic feature points. Then, using the relative motion reference between adjacent image frames of the carrier provided by IMU pre-integration, the candidate dynamic feature points are verified for inertial-visual geometric consistency, and the motion consistency residual between the inertial prediction result and the actual visual observation result is calculated. Furthermore, by combining the historical motion residual of the feature points and the number of continuous tracking frames, a trajectory temporal credibility model is established to obtain the static confidence of the feature points' trajectory. On this basis, the semantic dynamic probability, inertial-visual motion consistency residual, and trajectory static confidence are fused to form the real-time credibility evaluation result of the feature points, and it is mapped to the continuous weight of the back-end visual reprojection residual. Meanwhile, this invention evaluates the quality of visual observation based on information such as the number of effective feature points, feature space distribution, illumination stability, and image sharpness in the current image, and dynamically adjusts the relative weights of backend visual constraints and inertial constraints based on the visual quality evaluation results. This allows the system to make full use of visual information when visual observation is reliable, appropriately reduce the impact of unreliable visual observation when dynamic interference increases or visual quality degrades, and enhance the supporting role of inertial constraints in state estimation.

[0100] Through the above methods, this invention forms a dynamic scene robust estimation process consisting of dynamic candidate feature screening at the semantic level, inertial independence and consistency verification at the geometric level, trajectory stability evaluation at the temporal level, and continuous residual weighting and constraint adaptive scheduling at the optimization level. This process can effectively reduce the interference of dynamic visual observation on VIO system state estimation and improve the positioning accuracy, operational stability, and robustness of the system in urban roads and other highly dynamic environments.

[0101] The background section of this invention may include background information about the problems or circumstances surrounding the invention, rather than a description of prior art by others. Therefore, the content included in the background section is not an admission of prior art by the applicant.

[0102] The above description provides a further detailed explanation of the present invention in conjunction with specific / preferred embodiments, and it should not be construed that the specific implementation of the present invention is limited to these descriptions. For those skilled in the art, various substitutions or modifications can be made to these described embodiments without departing from the concept of the present invention, and all such substitutions or modifications should be considered within the scope of protection of the present invention. In the description of this specification, the reference to terms such as "an embodiment," "some embodiments," "preferred embodiment," "example," "specific example," or "some examples," etc., indicates that the specific features, structures, materials, or characteristics described in connection with that embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described can be combined in any suitable manner in one or more embodiments or examples. Furthermore, those skilled in the art can combine and integrate different embodiments or examples and features of different embodiments or examples described in this specification without contradiction. Although the embodiments of the present invention and their advantages have been described in detail, it should be understood that various changes, substitutions, and modifications can be made herein without departing from the scope defined by the appended claims.

Claims

1. A pose estimation method, characterized by, Includes the following steps: S1: Perform semantic segmentation on the image frame to obtain pixel-level semantic information, and generate a set of candidate dynamic feature points based on a predefined set of dynamic categories; S2: Use IMU pre-integration to obtain inertial motion prediction between adjacent image frames, and calculate the motion consistency residual between the inertial predicted pixel coordinates and the actual observed pixel coordinates of candidate dynamic feature points; S3: For feature point trajectories with historical data, calculate the static confidence level of the trajectory based on the historical sequence of the motion consistency residual and the continuous tracking state; S4: Fuse the pixel-level semantic information, the motion consistency residual, and the trajectory static confidence to generate a dynamic risk score for the candidate dynamic feature points, and map the dynamic risk score to a feature-level continuous soft weight of the visual reprojection residual. S5: Calculate the visual quality score of the current image frame and map the visual quality score to the frame-level overall weight of the visual reprojection residual; S6: Combine the feature-level continuous soft weights and the frame-level overall weights to jointly optimize the visual reprojection residuals and IMU pre-integration residuals to estimate the carrier pose.

2. The pose estimation method of claim 1, wherein, Step S1 specifically includes: Image frames are input into a semantic segmentation network to obtain pixel-level semantic category probability maps; Extract feature points from image frames and query the semantic category probability of the location of each feature point; Feature points whose maximum predicted probability belongs to a predefined dynamic category set and whose probability value exceeds a preset semantic filtering threshold are included in the candidate dynamic feature point set.

3. The pose estimation method of claim 1, wherein, The semantic segmentation network adopts the PIDNet structure, including proportional branches, integral branches, and differential branches, and outputs a pixel-level semantic category probability map through a boundary-guided fusion module.

4. The pose estimation method of claim 1, wherein, Step S2 specifically includes: The relative pose transformation between cameras in adjacent frames is derived using IMU pre-integration. By using binocular parallax or depth information, feature points are back-projected from the image coordinate system to the camera coordinate system to obtain the three-dimensional coordinates of the feature points; The three-dimensional coordinates are projected onto the next frame image plane according to the relative pose transformation to obtain the pixel coordinates predicted by inertia; The Euclidean distance between the inertial prediction pixel coordinates and the actual pixel coordinates obtained from feature matching is calculated and used as the motion consistency residual.

5. The pose estimation method according to claim 1, characterized in that, Step S3 specifically includes: For each feature point trajectory, maintain a first-in-first-out historical residual queue to store motion consistency residuals from recent frames; Calculate the average value of the motion consistency residuals in the historical residual queue; The static confidence level of the trajectory is calculated based on the average value and the number of consecutive tracking frames of the trajectory.

6. The pose estimation method according to claim 1, characterized in that, Step S4 specifically includes: The motion consistency residual is normalized to a preset range; The normalized motion consistency residual, the semantic dynamic confidence obtained from pixel-level semantic information, and the trajectory static confidence are weighted and fused to obtain the dynamic risk score. The dynamic risk score is converted into a continuous soft weight between 0 and 1 using a monotonically decreasing mapping function.

7. The pose estimation method according to claim 1, characterized in that, Step S5 evaluates the visual quality of the current image frame, specifically including: Calculate the ratio of the number of effectively tracked feature points in the current image frame to the number of reference points to obtain a quantity score; The image is divided into planar grids, and the entropy values ​​of feature points distributed in different grids are calculated to obtain the distribution uniformity score. Based on the semantic segmentation results, potential dynamic target regions are excluded, and the gray-level distribution difference between the current frame and the previous frame static image region is calculated to obtain the cross-frame photometric stability score. The image sharpness score is obtained by calculating the ratio of the variance of the Laplacian response of the image to the reference variance. The quantity score, distribution uniformity score, cross-frame photometric stability score, and image sharpness score are weighted or combined to evaluate the visual quality of the current image frame.

8. The pose estimation method according to claim 1, characterized in that, In step S5, the frame-level overall weight of the visual reprojection residual is adjusted linearly or non-linearly between a preset low threshold and a high threshold based on the visual quality score.

9. The pose estimation method according to claim 1, characterized in that, Step S6 specifically includes: in the sliding window optimization, using the feature-level continuous soft weights and the frame-level overall weights to weight the visual reprojection residuals, and combining the IMU pre-integration residuals and the marginalized prior residuals, to perform joint optimization to estimate the carrier pose.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the steps of the pose estimation method as described in any one of claims 1 to 9.