Pose estimation method and system based on complex dynamic scene, and storage medium

Abstract

The application discloses a pose estimation method and system based on a complex dynamic scene and a storage medium, and the pose estimation method comprises the following steps: S1, feature points in an input image are extracted, and a motion consistency detection is performed on a motion state of an object by using a sparse optical flow pyramid method or an epipolar geometry method to obtain a motion consistency detection result; S2, real-time semantic segmentation is performed on the input image by using a lightweight semantic segmentation model to obtain a semantic segmentation result; S3, semantic label classification is performed on the object in the scene according to the semantic segmentation result, and potential dynamic object detection is performed according to the semantic label classification result and a corresponding depth image to obtain a potential dynamic object detection result; S4, dynamic points are adaptively removed based on geometric information and semantic information; and S5, pose estimation is performed based on the reserved feature points. The application can effectively guarantee the accuracy and real-time performance of the pose estimation of a visual odometer in a complex dynamic scene.

Description

Technical Field

[0001] This invention belongs to the technical field of robot autonomous localization and navigation, specifically relating to a pose estimation method, system, and storage medium based on complex dynamic scenes. Background Technology

[0002] Visual odometry, a crucial component of visual SLAM (Simultaneous Localization and Mapping) systems, primarily utilizes image sequences acquired by a camera to calculate camera pose, thereby estimating motion trajectories. Visual odometry has been widely applied in fields such as autonomous robotics, augmented reality, and virtual reality.

[0003] Existing visual odometry systems can be mainly divided into three categories: 1) geometry-based visual odometry systems, 2) deep learning-based visual odometry systems, and 3) semantic-based visual odometry systems. Among these, existing geometry-based and deep learning-based visual odometry systems are difficult to use in dynamic scenes due to algorithmic limitations. In contrast, existing semantic-based visual odometry systems combine geometry-based visual odometry with semantic segmentation models. This allows for the identification of potential dynamic objects using semantic segmentation, and the removal of feature points from dynamic objects based on semantic labels, thus ensuring a certain degree of accuracy in visual odometry pose estimation. Therefore, semantic-based visual odometry systems exhibit high robustness to dynamic scenes and can meet the pose estimation requirements in typical dynamic scenarios. Due to the superior performance of semantic-based visual odometry systems in dynamic scenes, they have become the mainstream approach for visual odometry in dynamic environments.

[0004] Existing semantic-based visual odometry systems struggle to achieve high-precision and robust camera pose estimation in complex, dynamic scenes. There are three main reasons for this:

[0005] First, the over-reliance on semantic segmentation results to remove dynamic feature points fails to adequately integrate the true motion information provided by geometric data. On one hand, semantic segmentation has limited generalization ability, achieving only relatively accurate segmentation within the scope of the dataset. Therefore, dynamic feature points are still retained even when semantic segmentation fails to identify them. On the other hand, objects identified by semantic segmentation are often potential dynamic objects, leading to the removal of some high-quality static points. Both of these factors contribute to a decrease in camera pose estimation accuracy.

[0006] Second, the impact of image motion blur on downstream tasks, such as feature point extraction, semantic segmentation, and motion consistency detection, was not considered. These algorithms rely on high-quality images to ensure computational accuracy. Ultimately, image motion blur affects the accuracy of subsequent pose estimation.

[0007] Finally, most existing semantic-based visual odometry systems employ semantic segmentation models with high algorithmic complexity. While these models can guarantee the accuracy of semantic segmentation, they severely reduce the concurrency of the visual odometry system, making it difficult for visual odometry calculation methods to run in real time.

[0008] Therefore, to address the aforementioned technical problems, it is necessary to provide a pose estimation method, system, and storage medium based on complex dynamic scenes. Summary of the Invention

[0009] In view of this, the purpose of this invention is to provide a pose estimation method, system and storage medium based on complex dynamic scenes, so as to ensure the accuracy and real-time performance of pose estimation in complex dynamic scenes.

[0010] To achieve the above objectives, an embodiment of the present invention provides the following technical solution:

[0011] A pose estimation method based on complex dynamic scenes, the pose estimation method includes the following steps:

[0012] S1. Extract feature points from the input image and perform motion consistency detection on the motion state of the object using the sparse optical flow pyramid method or the epipolar geometry method to obtain the motion consistency detection result.

[0013] S2. Perform real-time semantic segmentation on the input image using a lightweight semantic segmentation model to obtain the semantic segmentation results;

[0014] S3. Based on the semantic segmentation results, perform semantic label classification on the objects in the scene, and perform potential dynamic object detection based on the semantic label classification results and the corresponding depth images to obtain the potential dynamic object detection results.

[0015] S4. Adaptively remove dynamic points based on geometric and semantic information, wherein the geometric information includes motion consistency detection results and potential dynamic object detection results, and the semantic information includes semantic segmentation results;

[0016] S5. Perform pose estimation based on the preserved feature points.

[0017] In one embodiment, the method further includes the following steps before steps S1 and S2:

[0018] Convert the input image to an RGB image; and / or,

[0019] The input image is preprocessed, including one or more of motion blur recovery preprocessing, histogram balancing preprocessing, and gamma correction preprocessing.

[0020] In one embodiment, in step S1:

[0021] Feature points are extracted using the ORB feature extraction algorithm, SIFT feature extraction algorithm, or SURF feature extraction algorithm.

[0022] In one embodiment, the motion consistency detection of the object's motion state using the sparse optical flow pyramid method in step S1 specifically involves:

[0023] Divide two consecutive images into multi-level pyramid models;

[0024] Sampling of sparse feature points is performed on the segmented image;

[0025] Optical flow is calculated based on feature points to obtain motion information of the sampled feature points;

[0026] The motion consistency detection of the object's motion state using the epipolar geometry method in step S1 is specifically as follows:

[0027] The fundamental matrix F between adjacent frames is calculated using the eight-point method;

[0028] The distance from the feature point to the corresponding epipolar line is calculated using the following formula:

[0029]

[0030] l = [A, B, C] T =FP1,

[0031] Where d is the distance from a feature point to the corresponding epipolar line, and P2 = [u2, v2, 1] T Let P1 = [u1, v1, 1] be the current feature point. T Let P2 be the feature point corresponding to P2 in the previous frame, u and v be the horizontal and vertical coordinates of the feature point in the image, respectively, and l be the epipolar line of the feature point P1 that matches the current feature point P2 in the previous frame in the current frame.

[0032] Determine the value of d. If d is less than a preset distance threshold, the feature point is considered to be a static object; otherwise, the feature point is considered to be a dynamic object.

[0033] In one embodiment, step S2 is followed by:

[0034] The semantic segmentation result is subjected to semantic inflation to obtain the semantically inflated result S. out for:

[0035]

[0036] Among them, S out I is the semantic segmentation result image after semantic dilation processing. SThe input is the semantic segmentation result image, and S is the convolution template or convolution kernel.

[0037] In one embodiment, step S3, which involves semantically labeling objects in the scene based on the semantic segmentation results, specifically includes:

[0038] Based on the semantic segmentation results, objects in the scene are classified into high-confidence dynamic objects, low-confidence dynamic objects, high-confidence static objects, or low-confidence static objects.

[0039] In step S3, the detection of potential dynamic objects based on the semantic label classification results and the corresponding depth images specifically involves:

[0040] Based on the semantic label classification results and the corresponding depth images, calculate the relationship value T between high-confidence dynamic objects and low-confidence dynamic objects. GS The relationship value T between high-confidence dynamic objects and low-confidence static objects. SG for:

[0041] T GS / T SG =αL depth +(1-α)L distance ;

[0042] Among them, α serves as an indicator of depth and distance information, and L depth This represents the depth difference, i.e., L. depth =L depthG -L depthS , where L depthG L represents the average depth of a high-confidence dynamic object. depthS L represents the average depth value of a low-confidence dynamic object or a low-confidence static object. distance The Euclidean distance between the centroid coordinates is... Among them, C G C represents the centroid coordinates of a high-confidence dynamic object. S The centroid coordinates of low-confidence dynamic or low-confidence static objects;

[0043] Get threshold T S for:

[0044]

[0045] Among them, S image The area of ​​the semantic segmentation result. For the average depth difference, L depthG_i Let L be the depth value of the i-th high-confidence dynamic object. depthS_jLet be the depth value of the j-th low-confidence dynamic object or low-confidence static object, N be the number of categories of high-confidence dynamic objects, and M be the number of categories of low-confidence dynamic objects or low-confidence static objects. For the average Euclidean distance, L distance_i Let N be the sum of distances between the i-th high-confidence dynamic object and all low-confidence dynamic objects or low-confidence static objects, where N is the number of categories of high-confidence dynamic objects and M is the number of categories of low-confidence dynamic objects or low-confidence static objects.

[0046] Comparison relation value T GS Relationship value T SG With threshold T S The size of T GS <T S or T SG <T S At that time, the feature point is identified as a potential moving object.

[0047] In one embodiment, step S4 includes:

[0048] Obtain the total number of feature points R1 from the motion consistency detection results, semantic segmentation results, and potential dynamic object detection results.

[0049] If R1 > 0, then adaptively remove dynamic points; otherwise, directly output the feature points extracted in step S1.

[0050] The adaptive removal of dynamic points is specifically as follows:

[0051] If R1 > L, then the corresponding dynamic feature points are removed based on the motion consistency detection results;

[0052] If M < R1 ≤ L, then the corresponding dynamic feature points are removed based on the motion consistency detection results and semantic segmentation results;

[0053] If S < R1 ≤ M, then the corresponding dynamic feature points are removed based on the motion consistency detection results, semantic segmentation results, and potential high-dynamic object detection results.

[0054] If R1≤S, then remove the corresponding dynamic feature points based on all results in R1;

[0055] Determine the value of the result R2 after adaptive removal of dynamic points and the threshold T. If R2 < T, return and re-execute the adaptive removal of dynamic points; otherwise, output the result R2 after adaptive removal of dynamic points.

[0056] Among them, L, M, S, and T satisfy S < M < L < T.

[0057] In one embodiment, step S5 specifically includes:

[0058] The EPNP algorithm is used to solve for the rotational motion R and translational motion t of the retained feature points, thus obtaining the pose information {R, t}.

[0059] Another embodiment of the present invention provides the following technical solution:

[0060] A pose estimation system based on complex dynamic scenes, the pose estimation system comprising:

[0061] The motion consistency detection module is used to extract feature points from the input image and perform motion consistency detection on the motion state of the object using the sparse optical flow pyramid method or the epipolar geometry method to obtain the motion consistency detection result.

[0062] The semantic segmentation module is used to perform real-time semantic segmentation on the input image using a lightweight semantic segmentation model to obtain the semantic segmentation result.

[0063] The semantic label classification module is used to classify objects in a scene using semantic labels based on the semantic segmentation results.

[0064] The latent dynamic object detection module is used to detect latent dynamic objects based on the semantic label classification results and the corresponding depth image, and obtain the latent dynamic object detection results.

[0065] An adaptive dynamic point removal module is used to adaptively remove dynamic points based on geometric and semantic information. The geometric information includes motion consistency detection results and potential dynamic object detection results, and the semantic information includes semantic segmentation results.

[0066] The pose estimation module is used to estimate pose based on the preserved feature points.

[0067] Another embodiment of the present invention provides the following technical solution:

[0068] A machine-readable storage medium storing executable instructions that, when executed, cause the machine to perform the pose estimation method described above.

[0069] The present invention has the following beneficial effects:

[0070] This invention first proposes a targeted visual odometry framework to address the problems associated with complex dynamic scenes; secondly, it fully considers the importance of semantic segmentation for the real-time performance of semantic-based visual odometry, and therefore adopts a lightweight semantic segmentation model to ensure the concurrency and real-time performance of the algorithm; finally, considering the complexity of the scene, it proposes an adaptive dynamic point culling algorithm based on multi-path geometric-semantic information to ensure the accuracy of pose estimation. Attached Figure Description

[0071] To more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the drawings in the following description are only some embodiments recorded in the present application. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0072] Figure 1 It is a schematic flowchart of a pose estimation method based on a complex dynamic scene in a specific embodiment of the present invention;

[0073] Figure 2 It is a schematic flowchart of adaptive dynamic point rejection in a specific embodiment of the present invention;

[0074] Figure 3 It is a schematic block diagram of a pose estimation system based on a complex dynamic scene in a specific embodiment of the present invention. Detailed implementation manners

[0075] In order to enable those skilled in the art to better understand the technical solutions in the present invention, the following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0076] To facilitate the understanding of the embodiments of the present invention, several elements introduced in the description of the embodiments of the present invention will be introduced here first.

[0077] SLAM: Simultaneous Localization and Mapping.

[0078] PnP: Perspective-n-Point, which solves the pose of the camera when given n 3D space points and their projection positions.

[0079] SVD decomposition: Singular Value Decomposition.

[0080] ICP: Iterative Closest Point, which estimates the relative pose between cameras using the three-dimensional coordinates of n pairs of feature points in different camera coordinate systems.

[0081] Refer Figure 1 As shown, a pose estimation method based on a complex dynamic scene in a specific embodiment of the present invention includes the following steps:

[0082] S1. Extract feature points from the input image and perform motion consistency detection on the motion state of the object using the sparse optical flow pyramid method or the epipolar geometry method to obtain the motion consistency detection result.

[0083] S2. Perform real-time semantic segmentation on the input image using a lightweight semantic segmentation model to obtain the semantic segmentation results;

[0084] S3. Based on the semantic segmentation results, perform semantic label classification on the objects in the scene, and perform potential dynamic object detection based on the semantic label classification results and the corresponding depth images to obtain the potential dynamic object detection results.

[0085] S4. Adaptively remove dynamic points based on geometric and semantic information, wherein the geometric information includes motion consistency detection results and potential dynamic object detection results, and the semantic information includes semantic segmentation results;

[0086] S5. Perform pose estimation based on the preserved feature points.

[0087] Preferably, the method further includes the following steps before steps S1 and S2:

[0088] Convert the input image to an RGB image; and

[0089] The input image is preprocessed, including one or more of motion blur recovery preprocessing, histogram balancing preprocessing, and gamma correction preprocessing.

[0090] Step S2 is followed by:

[0091] Semantic inflation is performed on the semantic segmentation results.

[0092] The following describes this embodiment in detail with reference to each step of the image restoration method.

[0093] This invention primarily employs two threads: a tracing thread and a semantic segmentation thread. In this embodiment, the invention will be described through the flow of these threads.

[0094] 1. The tracking thread receives RGB images and preprocesses them using an image preprocessing module. This preprocessing includes algorithms such as motion blur recovery, histogram balancing, and gamma correction. This results in a high-quality image, reducing the impact of the input image quality on the accuracy of subsequent algorithms.

[0095] When the input image is not an RGB image, it can be converted to an RGB image first, and then image preprocessing can be performed.

[0096] 2. The tracking thread extracts feature points from the obtained high-quality image using a feature extraction algorithm. ORB, SIFT, or SURF feature extraction algorithms can be used for efficient feature point extraction. After obtaining the extracted feature points, a motion consistency detection algorithm is used to detect the object's motion state. Sparse pyramid optical flow or epipolar geometry methods can be used to detect motion information.

[0097] Specifically, the sparse optical flow pyramid method is as follows:

[0098] Divide two consecutive images into multi-level pyramid models;

[0099] Sampling of sparse feature points is performed on the segmented image;

[0100] Optical flow is calculated based on feature points, thereby obtaining motion information of the sampled feature points.

[0101] Specifically, the polar geometry method is as follows:

[0102] The fundamental matrix F between adjacent frames is calculated using the eight-point method;

[0103] The distance from the feature point to the corresponding epipolar line is calculated using the following formula:

[0104]

[0105] l = [A, B, C]T = FP1,

[0106] Where d is the distance from a feature point to the corresponding epipolar line, and P2 = [u2, v2, 1] T Let P1 = [u1, v1, 1] be the current feature point. T Let P2 be the feature point corresponding to P2 in the previous frame, u and v be the horizontal and vertical coordinates of the feature point in the image, respectively, and l be the epipolar line of the feature point P1 that matches the current feature point P2 in the previous frame in the current frame.

[0107] Finally, the size of d is determined. If d is less than the preset distance threshold, the feature point is considered to be a static object; otherwise, the feature point is considered to be a dynamic object.

[0108] 3. Input the preprocessed high-quality RGB image into the semantic segmentation thread for real-time semantic segmentation processing using a lightweight semantic segmentation model to obtain the semantic segmentation result.

[0109] Semantic segmentation, a fundamental area of ​​computer vision, involves labeling each pixel in an image with its corresponding semantic tag. Existing semantic segmentation models based on convolutional neural networks (CNNs) have demonstrated excellent performance in terms of accuracy and speed after years of development, meeting the requirements of this invention. Therefore, this invention employs a lightweight semantic segmentation model based on CNNs to achieve real-time semantic segmentation processing. Existing lightweight semantic segmentation models based on CNNs, such as the DeepLab series, SegNet, and PP-LiteSeg, can be used.

[0110] Then, by using the semantic inflation algorithm, the uncertainty of the boundary of moving objects can be reduced and the impact of motion blur on the accuracy of subsequent algorithms can be reduced.

[0111] The result S after semantic inflation out for:

[0112]

[0113] Among them, S out I is the semantic segmentation result image after semantic dilation processing. S The input is the semantic segmentation result image, and S is the convolution template or convolution kernel, which can be square or circular.

[0114] Next, to fully utilize the semantic segmentation thread and increase system concurrency, this invention proposes a semantic label classification algorithm and a latent moving object detection algorithm within the semantic segmentation thread, further reducing the impact of latent moving objects on pose estimation accuracy. The semantic label classification algorithm categorizes objects in the scene into four classes: "high-confidence dynamic objects," "low-confidence dynamic objects," "high-confidence static objects," and "low-confidence static objects." As shown in Table 1, specific examples are provided. This table can be customized according to the scene. Here, the semantic label classification algorithm will process the input semantic segmentation result S... out The corresponding objects on the list are classified to obtain semantic label classification results.

[0115] Table 1. Semantic Tag Classification Diagram

[0116] Semantic tag categories Corresponding object High-confidence dynamic objects Humans, cats, dogs, etc. Low-confidence dynamic objects Chairs, books, pens, etc. High confidence static objects Pillars, ground, etc. Low confidence static objects Tables, cabinets, shelves, etc.

[0117] The latent dynamic object detection algorithm mainly calculates the relationship value T between "high-confidence dynamic objects" and "low-confidence dynamic objects" based on the semantic label classification results and the corresponding depth images. GS The relationship value T between "high-confidence dynamic objects" and "low-confidence static objects". SG .

[0118] Here, we first extract the centroid coordinates C of each object region after classification. The formula for calculating the centroid coordinates C is as follows:

[0119]

[0120]

[0121]

[0122]

[0123] Where x and y represent the pixel coordinates of the object region after classification according to semantic labels, I(x,y) is the pixel value of the region at (x,y), and m 00 Let m be the zeroth moment of the object region after classification based on semantic labels. 01 m 01 These are the first moments of the object region in the y and x directions, respectively, after classification based on semantic labels.

[0124] Then, based on the semantic label classification results and the corresponding depth images, the relationship value T between high-confidence dynamic objects and low-confidence dynamic objects is calculated. GS The relationship value T between high-confidence dynamic objects and low-confidence static objects. SG for:

[0125] T GS / T SG =αL depth +(1-α)L distance ;

[0126] Among them, α serves as an indicator of depth and distance information, and L depth This represents the depth difference, i.e., L. depth =L depthG -L depthS , where L depthG L represents the average depth of a high-confidence dynamic object. depthS L represents the average depth value of a low-confidence dynamic object or a low-confidence static object. distance The Euclidean distance between the centroid coordinates is... Among them, C G C represents the centroid coordinates of a high-confidence dynamic object. S The centroid coordinates of low-confidence dynamic or low-confidence static objects.

[0127] Next, in order to adaptively discriminate the calculated T GS Or T SGIs there a situation where a low-confidence dynamic object or a low-confidence static object is an actual dynamic object? The present invention obtains a threshold T through an algorithm combining the input image size, average depth, and average distance. S It is:

[0128]

[0129] Where S image is the area of the semantic segmentation result, is the average depth difference, L depthG_i is the depth value of the i-th high-confidence dynamic object, L depthS_j is the depth value of the j-th low-confidence dynamic object or low-confidence static object, N is the number of categories of high-confidence dynamic objects, M is the number of categories of low-confidence dynamic objects or low-confidence static objects, is the average Euclidean distance, L distance_i is the sum of the distances from the i-th high-confidence dynamic object to all low-confidence dynamic objects or low-confidence static objects, N is the number of categories of high-confidence dynamic objects, M is the number of categories of low-confidence dynamic objects or low-confidence static objects.

[0130] Compare the relational value T GS , the relational value T SG with the threshold T S . When T GS < T S or T SG < T S , it is determined that this feature point belongs to a potential moving object. Among them, the potential moving object detection result is divided into a high-dynamic object detection result and a low-dynamic object detection result according to T GS and T SG .

[0131] 4. The tracking thread inputs the calculated motion consistency detection result, semantic segmentation result, and potential moving object detection result into the adaptive dynamic point elimination algorithm for adaptive dynamic point elimination. Here, the motion consistency detection result and the potential moving object detection result are classified as geometric information, and the semantic segmentation result is classified as semantic information.

[0132] Refer Figure 2As shown, the adaptive dynamic point removal in this embodiment proposes a multi-channel geometric-semantic information processing method, which can effectively select the corresponding dynamic point removal algorithm according to different scenarios, that is, it can effectively ensure the accuracy of visual odometry pose estimation and the robustness in high-complexity and high-dynamic scenarios. First, in the multi-channel geometric-semantic information processing algorithm, the coordinate points obtained from semantic information and geometric information are used to calculate the first-stage result R1 of the adaptive dynamic point removal algorithm, that is, the result (the number of feature points) obtained by summing the geometric information (motion consistency detection result, potential dynamic object detection result) and the semantic information (semantic segmentation result), and it is used to judge whether to further execute. Then, if the first-stage result R1 indicates the existence of dynamic objects, that is, R1>0, the multi-channel geometric-semantic information processing algorithm will use a targeted method to remove low-quality dynamic feature points according to R1; otherwise, directly output the feature point result extracted in the second step for distance matching. That is, within the effective Euclidean distance of the coordinate points obtained from semantic information and geometric information, the feature points extracted in the second step are identified as low-quality dynamic feature points and removed.

[0133] Refer Figure 2 As shown, first obtain the total result R1 of all feature points in the motion consistency detection result, semantic segmentation result, and potential dynamic object detection result. If R1>0, perform adaptive removal of dynamic points; otherwise (that is, when R1 = 0), directly output the feature points extracted in step S1.

[0134] The adaptive removal of dynamic points is specifically as follows:

[0135] If R1>L, remove the corresponding dynamic feature points according to the motion consistency detection result;

[0136] If M<R1≤L, remove the corresponding dynamic feature points according to the motion consistency detection result and the semantic segmentation result;

[0137] If S<R1≤M, remove the corresponding dynamic feature points according to the motion consistency detection result, semantic segmentation result, and potential high-dynamic object detection result;

[0138] If R1≤S, remove the corresponding dynamic feature points according to all results in R1;

[0139] Judge the size of the result R2 after adaptive removal of dynamic points and the threshold T. If R2<T, return to re-execute the adaptive removal of dynamic points; otherwise, output the result R2 after adaptive removal of dynamic points;

[0140] Among them, L, M, S, and T satisfy S<M<L<T.

[0141] In this embodiment, the effective Euclidean distance is set to 20 pixels, and the corresponding thresholds are set to S = 60, M = 120, and L = 300 respectively. Next, when the result R2 of the selected rejection algorithm does not meet the requirement of the threshold T, that is, R2 < T, the algorithm will execute the rejection algorithm at the upper level to ensure that there are sufficient feature points for pose calculation in the subsequent process. Here, the threshold T is set to 400 in this embodiment.

[0142] 5. Estimate the pose {R, t} based on the remaining feature points.

[0143] Use the EPnP algorithm to solve the rotation motion R and the translation motion t by using the remaining high-quality feature points. The EPnP algorithm does not require iteration, does not require an initial estimate value, and can achieve high precision, meeting the requirements of real-time and high-precision.

[0144] Since the coordinates of the 3D points in the world coordinate system are known and their 2D projection points on the image, as well as the camera internal parameters. Therefore, it is necessary to transform the 2D points into 3D points in the camera coordinate system through the internal parameters Then use the ICP algorithm to solve the 3D-3D transformation to obtain the pose.

[0145] The specific process of the algorithm: First, define 4 control points for each coordinate system

[0146]

[0147] Among them,

[0148] Secondly, use the projection equation to restore the 3D points in the camera coordinate system from the image 2D points;

[0149] [[ID=3o]]

[0150] Among them, w i is the projection scale coefficient, u i is the 2D projection coordinate corresponding to the 3D reference point in the camera coordinate system and K is the camera internal parameter matrix.

[0151] Next, according to the matrix obtained above, use SVD decomposition and Gauss-Newton method to solve Finally, based on the coordinates of the obtained 3D points in the world coordinate system and the corresponding coordinates in the camera coordinate system The ICP algorithm can solve the pose {R, t}.

[0152] The present invention can effectively solve the problem that visual odometry is difficult to work efficiently in complex dynamic scenes. Since the existing visual odometry algorithms in dynamic scenes rely heavily on the results of semantic segmentation, when dynamic objects are not recognized by semantic segmentation or the dynamic objects considered by semantic labels are stationary, the pose estimation accuracy of visual odometry in complex dynamic scenes will be greatly reduced.

[0153] Therefore, the present invention first proposes a targeted visual odometry framework for the problems existing in complex dynamic scenes; secondly, fully considering the importance of semantic segmentation for the real-time performance of semantic-based visual odometry algorithms, a lightweight semantic segmentation model is adopted to ensure the concurrency and real-time performance of the algorithm; finally, considering the complexity of the scene, an adaptive dynamic point elimination algorithm based on multi-channel geometric-semantic information is proposed to ensure the accuracy of pose estimation.

[0154] The algorithm proposed by the present invention can effectively ensure the accuracy and real-time performance of pose estimation of visual odometry in complex dynamic scenes.

[0155] See Figure 3 As shown, in a specific embodiment of the present invention, a pose estimation system based on a complex dynamic scene includes:

[0156] A motion consistency detection module, configured to extract feature points in the input image and perform motion consistency detection on the motion state of the object through the sparse optical flow pyramid method or the epipolar geometry method to obtain a motion consistency detection result;

[0157] A semantic segmentation module, configured to perform real-time semantic segmentation on the input image through a lightweight semantic segmentation model to obtain a semantic segmentation result;

[0158] A semantic label classification module, configured to classify the semantic labels of the objects in the scene according to the semantic segmentation result;

[0159] A potential dynamic object detection module, configured to perform potential dynamic object detection according to the semantic label classification result and the corresponding depth image to obtain a potential dynamic object detection result;

[0160] An adaptive dynamic point elimination module, configured to adaptively eliminate dynamic points based on geometric information and semantic information, where the geometric information includes the motion consistency detection result and the potential dynamic object detection result, and the semantic information includes the semantic segmentation result;

[0161] A pose estimation module, configured to perform pose estimation based on the retained feature points.

[0162] The present invention also discloses a machine-readable storage medium, which stores executable instructions that, when executed, cause the machine to execute the above-mentioned pose estimation method.

[0163] Specifically, a system or apparatus equipped with a readable storage medium may be provided, on which software program code implementing the functions of any of the embodiments described above is stored, and the computer or processor of the system or apparatus can read and execute the instructions stored in the readable storage medium.

[0164] In this case, the program code read from the readable medium itself can perform the functions of any of the above embodiments, and therefore the machine-readable code and the readable storage medium storing the machine-readable code constitute a part of this specification.

[0165] Examples of readable storage media include floppy disks, hard disks, magneto-optical disks, optical disks (such as CD-ROM, CD-R, CD-RW, DVD-ROM, DVD-RAM, DVD-RW, DVD-RW), magnetic tapes, non-volatile memory cards, and ROMs. Alternatively, program code can be downloaded from a server computer or the cloud via a communication network.

[0166] Those skilled in the art will understand that the various embodiments disclosed above can be modified and varied without departing from the spirit of the invention. Therefore, the scope of protection of this specification should be defined by the appended claims.

[0167] It should be noted that not all steps and units in the above process and system structure diagrams are mandatory; some steps or units can be omitted according to actual needs. The execution order of each step is not fixed and can be determined as needed. The device structure described in the above embodiments can be a physical structure or a logical structure. That is, some units may be implemented by the same physical client, or some units may be implemented by multiple physical clients, or they may be jointly implemented by certain components in multiple independent devices.

[0168] In the above embodiments, the hardware units or modules can be implemented mechanically or electrically. For example, a hardware unit, module, or processor may include permanent dedicated circuitry or logic (such as a dedicated processor, FPGA, or ASIC) to perform the corresponding operation. The hardware unit or processor may also include programmable logic or circuitry (such as a general-purpose processor or other programmable processor), which can be temporarily configured by software to perform the corresponding operation. The specific implementation method (mechanical, dedicated permanent circuitry, or temporarily configured circuitry) can be determined based on cost and time considerations.

[0169] The specific embodiments described above with reference to the accompanying drawings are exemplary embodiments, but do not represent all embodiments that can be implemented or fall within the scope of the claims. The term "exemplary" as used throughout this specification means "serving as an example, instance, or illustration" and does not imply that it is "preferred" or "advantageous" compared to other embodiments. Specific details are included to provide an understanding of the described techniques. However, these techniques can be practiced without these specific details. In some instances, well-known structures and apparatuses are shown in block diagram form to avoid obscuring the concepts of the described embodiments.

[0170] The foregoing description of this disclosure is provided to enable any person skilled in the art to implement or use this disclosure. Various modifications to this disclosure will be apparent to those skilled in the art, and the general principles applicable herein can be applied to other variations without departing from the scope of this disclosure. Therefore, this disclosure is not limited to the examples and designs described herein, but is consistent with the widest scope of the principles and novel features disclosed herein.

Claims

1. A pose estimation method based on complex dynamic scenes, characterized in that, The pose estimation method includes the following steps: S1. Extract feature points from the input image and perform motion consistency detection on the motion state of the object using the sparse optical flow pyramid method or the epipolar geometry method to obtain the motion consistency detection result. S2. Perform real-time semantic segmentation on the input image using a lightweight semantic segmentation model to obtain the semantic segmentation results; S3. Based on the semantic segmentation results, perform semantic label classification on the objects in the scene, and perform potential dynamic object detection based on the semantic label classification results and the corresponding depth images to obtain the potential dynamic object detection results. S4. Adaptively remove dynamic points based on geometric and semantic information, wherein the geometric information includes motion consistency detection results and potential dynamic object detection results, and the semantic information includes semantic segmentation results; S5. Perform pose estimation based on the preserved feature points; In step S3, the semantic label classification of objects in the scene based on the semantic segmentation results is specifically as follows: Based on the semantic segmentation results, objects in the scene are divided into high-confidence dynamic objects, low-confidence dynamic objects, high-confidence static objects, and low-confidence static objects. In step S3, the detection of potential dynamic objects based on the semantic label classification results and the corresponding depth images is specifically as follows: The relationship value between high-confidence dynamic objects and low-confidence dynamic objects is calculated based on the semantic label classification results and the corresponding depth images. T GS for: ； in, As an indicator of depth and distance information L depth Indicates the depth difference, i.e. ,in, L depthG This represents the average depth value of a high-confidence dynamic object. L depthS This represents the average depth value of a low-confidence dynamic object. L distance The Euclidean distance between the centroid coordinates is... ,in, C G The coordinates of the centroid of a high-confidence dynamic object. C S The centroid coordinates of a low-confidence dynamic object; The relationship between high-confidence dynamic objects and low-confidence static objects is calculated based on the semantic label classification results and the corresponding depth images. T SG for: ； in, As an indicator of depth and distance information L depth Indicates the depth difference, i.e. ,in, L depthG This represents the average depth value of a high-confidence dynamic object. L depthS This represents the average depth value of a low-confidence static object. L distance The Euclidean distance between the centroid coordinates. ,in, C G The centroid coordinates of a high-confidence dynamic object. C S The centroid coordinates of a low-confidence static object; Get threshold T S for: ； in, The area of ​​the semantic segmentation result. The average depth difference For the first i Depth value of a high-confidence dynamic object. For the first j The depth values ​​of low-confidence dynamic objects or low-confidence static objects, where N is the number of categories of high-confidence dynamic objects and M is the number of categories of low-confidence dynamic objects or low-confidence static objects. The average Euclidean distance. For the first i The sum of distances between a high-confidence dynamic object and all low-confidence dynamic objects or low-confidence static objects. N This represents the number of categories for high-confidence dynamic objects. M The number of categories for low-confidence dynamic objects or low-confidence static objects; Comparison Relationship Values T GS Relationship value T SG With threshold T S The size, when T GS < T S or T SG < T S At that time, the feature point is identified as a potential moving object.

2. The pose estimation method based on complex dynamic scenes according to claim 1, characterized in that, Before steps S1 and S2, the following also applies: Convert the input image to an RGB image; and / or, The input image is preprocessed, including one or more of motion blur recovery preprocessing, histogram balancing preprocessing, and gamma correction preprocessing.

3. The pose estimation method based on complex dynamic scenes according to claim 1, characterized in that, In step S1: Feature points are extracted using the ORB feature extraction algorithm, SIFT feature extraction algorithm, or SURF feature extraction algorithm.

4. The pose estimation method based on complex dynamic scenes according to claim 1, characterized in that, In step S1, the motion consistency detection of the object's motion state using the sparse optical flow pyramid method is specifically as follows: Divide two consecutive images into multi-level pyramid models; Sampling of sparse feature points is performed on the segmented image; Optical flow is calculated based on feature points to obtain motion information of the sampled feature points; Step S1 involves detecting the motion consistency of the object using the epipolar geometry method, specifically as follows: Calculate the fundamental matrix between adjacent frames using the eight-point method F ; The distance from the feature point to the corresponding epipolar line is calculated using the following formula: ， ， in, d The distance from a given feature point to its corresponding epipolar line. For the current feature point, for P 2 The corresponding feature points in the previous frame, u and v These are the horizontal and vertical coordinates of the feature points in the image, respectively. l For the feature points in the previous frame and the current feature points P 2 Matching feature points P 1 In the current frame's epipolar line; judge d The size, if d If the distance is less than a preset threshold, the feature point is considered a static object; otherwise, the feature point is considered a dynamic object.

5. The pose estimation method based on complex dynamic scenes according to claim 1, characterized in that, Step S2 is followed by: The semantic segmentation results are subjected to semantic inflation processing to obtain the semantically inflated result. S out for: ； in, S out This is the semantic segmentation result image after semantic dilation processing. I S The input is the semantic segmentation result image. S It is the convolution template or convolution kernel.

6. The pose estimation method based on complex dynamic scenes according to claim 1, characterized in that, Step S4 includes: Obtain the total number of feature points R1 from the motion consistency detection results, semantic segmentation results, and potential dynamic object detection results. If R1 > 0, then adaptively remove dynamic points; otherwise, directly output the feature points extracted in step S1. The adaptive removal of dynamic points is specifically as follows: If R1 > L, then the corresponding dynamic feature points are removed based on the motion consistency detection results; If M < R1 ≤ L, then the corresponding dynamic feature points are removed based on the motion consistency detection results and semantic segmentation results; If S < R1 ≤ M, then the corresponding dynamic feature points are removed based on the motion consistency detection results, semantic segmentation results, and potential high-dynamic object detection results. If R1≤S, then remove the corresponding dynamic feature points based on all results in R1; Determine the value of the result R2 after adaptive removal of dynamic points and the threshold T. If R2 < T, return and re-execute the adaptive removal of dynamic points; otherwise, output the result R2 after adaptive removal of dynamic points. Among them, L, M, S, and T satisfy S < M < L < T.

7. The pose estimation method based on complex dynamic scenes according to claim 1, characterized in that, Step S5 is as follows: The rotational motion of the retained feature points is solved using the EPNP algorithm. R Translational movement t Obtain pose information { R , t } 8. A machine-readable storage medium storing executable instructions that, when executed, cause the machine to perform the pose estimation method as described in any one of claims 1 to 7.

Images