A pose estimation method based on multi-sensor multi-feature modular fusion

Abstract

The present application provides a posture estimation method based on multi-feature modular fusion of multiple sensors. The posture estimation method includes: step A1, determining the pixel coordinate system, the intrinsic parameters of the reference sensor and the extrinsic parameters of the additional sensor; step A2, performing dynamic area filtering on the image in the pixel coordinate system to obtain the image area of ​​the target feature; step A3-A5, based on the image area of ​​the target feature, respectively obtaining the edge feature residuals, key point feature residuals and depth normal feature residuals of the reference sensor, lidar and infrared camera and the corresponding Jacobian matrix. Step A6, through S2 T S1 , S3 T S1 Get the velocity matrix conversion relationship S2 V S1 and S3 V S1 , and get the velocity in the S1 coordinate system S1 v, Step A7, obtain the posture of the current target coordinate system relative to the reference sensor S1 through an iterative nonlinear minimization method S1 T o The proposed method effectively improves the accuracy and robustness of target tracking to adapt to a wider range of application scenarios.

Description

Technical Field

[0001] The present application belongs to the field of attitude measurement technology, and in particular relates to an attitude estimation method based on multi-feature modular fusion of multiple sensors. Background Art

[0002] Accurately locating the position of an object of interest relative to a sensor in three-dimensional space is a key step in introducing dynamic manipulation into robotic vision. This allows for the execution of complex tasks such as object grasping, mobile robot positioning, and augmented reality in real time in a closed loop through visual servoing, while accounting for dynamic environmental disturbances. The primary challenge in object tracking lies not only in tracking accuracy but also in achieving high reliability and robustness.

[0003] Model-based object tracking aims to compute the pose transformation of a target relative to the camera based on prior knowledge of the target's geometry. Its fundamental principle is to achieve pose estimation by minimizing the norm of the reprojection error using iterative nonlinear minimization techniques. When Gaussian noise is assumed in the measurement, the reprojection error can be minimized via maximum likelihood estimation. Early approaches considered models consisting of target edges as the basic measurement, but these approaches are likely to fail when the target or its background is rich in texture, and their robustness decreases sharply when certain edges become ambiguous. One approach to addressing this problem is to fuse information from edges, keypoints, and 3D point clouds to achieve both target tracking and pose estimation. Currently, a commonly used approach is based on visible light cameras, extracting edge and keypoint features of the target from visible light images and extracting depth features (i.e., point-to-surface distances) from 3D point clouds.

[0004] Existing solutions based on visible light cameras combine features based on visible light and point clouds, improving the robustness of target tracking to a certain extent. However, they have certain drawbacks: 1) In scenes with highly variable lighting conditions and rich background textures, image key points and certain edges can be easily ambiguous, leading to tracking failures; 2) the hardware requirements of visible light cameras limit the widespread use of this approach; and 3) the algorithm involves multiple parameters, making manual tuning cumbersome and difficult to guarantee optimal results. Summary of the Invention

[0005] The main purpose of the present invention is to provide a posture estimation method based on multi-sensor multi-feature modular fusion to solve the problems of insufficient robustness and limited scope of application of posture estimation methods in the prior art.

[0006] In order to solve the above problems, the present invention provides a posture estimation method based on multi-feature modular fusion of multiple sensors, wherein the posture estimation method includes:

[0007] Step A1: Determine the pixel coordinate system, the intrinsic parameters of the reference sensor, and the extrinsic parameters of the additional sensor, and transform the points in the additional sensor coordinate system to the reference sensor coordinate system, and then project the points in the reference sensor coordinate system to the pixel coordinate system. The reference sensor includes a visible light camera, and the additional sensors include a lidar and an infrared camera. The extrinsic parameters include the coordinate system transformation matrix of the lidar relative to the reference sensor. S2 T S1 and the coordinate system transformation matrix of the infrared camera relative to the reference sensor S3 T S1 ;

[0008] Step A2, performing dynamic area filtering on the image in the pixel coordinate system to obtain the image area of ​​the target feature;

[0009] Step A3: Based on the image area of ​​the target feature, obtain the edge feature residual ε of the reference sensor, lidar, and infrared camera respectively e1 , ε e2 , ε e2 , and the corresponding Jacobian matrix L e1 , L e2 , L e3 ;

[0010] Step A4: Based on the image area of ​​the target feature, obtain the key point feature residual ε of the reference sensor and the infrared camera respectively. p1 , ε p2 and the corresponding Jacobian matrix L p1 and L p2 ;

[0011] Step A5: Based on the image area of ​​the target feature, the depth normal feature residual ε of the reference sensor and the lidar are obtained respectively. d1 、e d2 and the corresponding Jacobian matrix L d1 , L d2 ;

[0012] Step A6, by S2 T S1 , S3 T S1 Get the velocity matrix conversion relationship of the laser radar and infrared camera relative to the reference sensor respectively S2 V S1 and S3 V S1 , and get the velocity in the S1 coordinate system S1 v, S1 The expression for v is

[0013]

[0014] Step A7, obtain the posture of the current target coordinate system relative to the reference sensor S1 through an iterative nonlinear minimization method S1 T o , the expression to be solved is

[0015] S1(k+1) T o =ΔT -1S1(k) T o 、

[0016] ΔT= S1(k) T S1(k+1) =exp( S1 v),

[0017] in, S1(k) T o is the pose of the target relative to the reference sensor at the kth iteration step, S1(k+1) T o is the pose of the target relative to the reference sensor at the k+1th iteration step, S1(k) T S1(k+1) is the relative pose of the reference sensor at the k+1 iteration step relative to the k iteration step.

[0018] Furthermore, the above step A2 includes:

[0019] Step A21, pre-processing two images I1 and I2 separated by several frames in time sequence, where I1 is the image later in time sequence and I2 is the image earlier in time sequence;

[0020] Step A22, dividing I1 and I2 into grid areas respectively;

[0021] Step A23, calculating the distance between the corresponding grids I1 and I2, and determining the grids with distance values ​​greater than a threshold as dynamic grids;

[0022] Step A24 : For the dynamic grid in I1 , the image region with the largest envelope rectangle is selected as the target feature.

[0023] Furthermore, the above preprocessing includes grayscale conversion and Gaussian smoothing.

[0024] Furthermore, the above-mentioned grid area division method includes:

[0025] The width and height are divided into the same number of grids n, or

[0026] Keep each grid square with a side length of m pixels.

[0027] Furthermore, the above-mentioned internal parameters include optical characteristic parameters.

[0028] Furthermore, the above step A3 includes:

[0029] Step A31: determine the edge feature p of the image area of ​​the target feature by using an image edge detection algorithm. i ;

[0030] Step A32: Determine the edge straight lines of the reference sensor and the additional sensor in the reference sensor coordinate system, and project them to the pixel coordinate system. The edge straight lines projected to the pixel coordinate system are expressed as xcosθ+ysinθ=ρ in the polar coordinate system.

[0031] Step A33, using ε e =ρ-(x e cosθ+y e sinθ) to get the residual, where x e and y e are the coordinates of the points on the edge line; the corresponding Jacobian matrix is

[0032]

[0033] where λ e =λ ρ +aλ θ ,λ ρ =(Aρcosθ+Bρsinθ+C) / D,λ θ =(Asinθ-Bcosθ) / D, α=x e sinθ-y e cosθ, A, B, C, and D define the 3D plane coordinate system where the detected edge line is located. The plane is defined in the reference sensor coordinate system.

[0034] Furthermore, the above step A4 includes a key point detection process, which includes a Harris detection algorithm and a KLT algorithm for tracking key points.

[0035] Furthermore, the above step A5 includes:

[0036] Step A51: Convert the target point obtained by the laser radar into the reference sensor coordinate system and then project it onto the pixel coordinate system to complete the alignment of the 3D point cloud and the image.

[0037] Step A52, step A52, the distance from the point to the surface is used as the depth normal feature residual ε d1 , ε d2 , and then get the corresponding Jacobian matrix L d1 、L d2 .

[0038] Furthermore, the additional sensor includes a binocular camera.

[0039] Furthermore, in the above step A7, K≥3.

[0040] By applying the solution of the present invention, the following effects are achieved.

[0041] (1) This application uses lidar sensor data to extract edge features and depth normal features, and uses infrared camera data to extract edge features and key point features. Through joint calibration, the relative position of the lidar and infrared camera relative to the visible light camera is obtained. Finally, the Jacobian matrix stacking method is used to fuse features with the image of the visible light camera. This solution combines the advantages of various sensors, especially in extreme scenarios such as insufficient lighting conditions and complex environmental backgrounds, to improve the accuracy and robustness of target tracking and posture estimation.

[0042] (2) This application proposes a dynamic area filtering scheme, which performs dynamic area filtering during the multi-sensor feature fusion process, removes the static redundant parts within the field of view, and extracts features only within the filtered dynamic area, thereby reducing the amount of calculation, shielding the background infection to a certain extent, and effectively improving the efficiency and robustness of target tracking. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] The following will further illustrate the above-mentioned characteristics, technical features, advantages and implementation methods of the present application in a clear and understandable manner through the description of preferred embodiments and in conjunction with the accompanying drawings. The following drawings are intended only to illustrate and explain the present application and do not limit the scope of the present application. Among them:

[0044] Figure 1 A flowchart of a method for posture estimation based on multi-sensor multi-feature modular fusion in one embodiment of the present application is shown;

[0045] Figure 2 A flow chart of dynamic area filtering in one embodiment of the present application is shown;

[0046] Figure 3 Two exemplary grid area division methods in one embodiment of the present application are shown;

[0047] Figure 4 A method for calculating the grid distance between two images in chronological order in one embodiment of the present application is shown. DETAILED DESCRIPTION

[0048] In order to have a clearer understanding of the technical features, purposes and effects of this application, the specific implementation methods of this application are now described with reference to the accompanying drawings.

[0049] As described in the background technology, in the prior art, the posture estimation method in the prior art has the problems of insufficient robustness and limited scope of application. In order to solve the above problems, Figure 1As shown, in a specific embodiment of the present application, a posture estimation method based on multi-feature modular fusion of multiple sensors is provided, wherein the posture estimation method includes:

[0050] Step A1: Determine the pixel coordinate system, the intrinsic parameters of the reference sensor, and the extrinsic parameters of the additional sensor, and transform the points in the additional sensor coordinate system to the reference sensor coordinate system, and then project the points in the reference sensor coordinate system to the pixel coordinate system. The reference sensor includes a visible light camera, and the additional sensors include a lidar and an infrared camera. The extrinsic parameters include the coordinate system transformation matrix of the lidar relative to the reference sensor. S2 T S1 and the coordinate system transformation matrix of the infrared camera relative to the reference sensor S3 T S1 ; Step A2, dynamic area filtering is performed on the image in the pixel coordinate system to obtain the image area of ​​the target feature; Step A3, based on the image area of ​​the target feature, the edge feature residual ε of the reference sensor, the lidar, and the infrared camera are obtained respectively e1 , ε e2 , σ e3 , and the corresponding Jacobian matrix L e1 , L e2 , L e3 ; Step A4, based on the image area of ​​the target feature, obtain the key point feature residual ε of the reference sensor and the infrared camera respectively p1 , ε p2 and the corresponding Jacobian matrix L p1 and L p2 ; Step A5, based on the image area of ​​the target feature, respectively obtain the depth normal feature residual ε of the reference sensor and the lidar d1 , ε d2 and the corresponding Jacobian matrix L d1 , L d2 ; Step A6, by S2 T S1 , S3 T S1 Get the velocity matrix conversion relationship of the laser radar and infrared camera relative to the reference sensor respectively S2 V S1 and S3 V S1 , and get the velocity in the S1 coordinate system S1 v, Step A7, obtain the posture of the current target coordinate system relative to the reference sensor S1 through an iterative nonlinear minimization method S1 T o .

[0051] This application proposes a posture estimation scheme based on multiple sensors such as visible light cameras, lidars and infrared cameras, which performs multi-source feature fusion in a modular decoupling manner, effectively improving the accuracy and robustness of target tracking to adapt to a wider range of application scenarios.

[0052] The technical solution of the present application is further explained below in conjunction with embodiments.

[0053] Step A1 (joint calibration):

[0054] The visible light camera (RGB) is used as the reference sensor (S1). The intrinsic parameters of the visible light camera and the extrinsic parameters of other sensors such as the lidar (S2) and infrared camera (S3) are obtained through calibration, that is, the coordinate system transformation matrix relative to the reference sensor. S2 T S1 , S3 T S1 And according to the coordinate system conversion matrix, the points on the coordinates of other sensors are converted and projected onto the image of the reference visible light camera. Taking the laser radar S2 as an example, first, point P in the S2 coordinate system is converted to s2 Convert to the S1 camera coordinate system as follows:

[0055]

[0056] Expand as follows:

[0057]

[0058] Through the S1 camera internal parameters, point P in the S1 camera coordinate system is S1 Projection to pixel coordinate system P = (x, y):

[0059]

[0060] Step A2 (dynamic area filtering, such as Figure 2 shown):

[0061] Step A21, grayscale and Gaussian smoothing are performed on two images I1 (back) and I2 (front) separated by n frames in time sequence;

[0062] Step A22, divide the image into grid areas. Set the width and height of the image to w and h respectively. There are two ways to divide the grid: the first is to divide the width and height into the same number of grids n, then the width and height of each grid are (like Figure 3 (A)). The second method is to keep each grid shape as a square with a side length of m pixels, then the number of grids is (like Figure 3 (as shown in (B)).

[0063] Step A23 (find the distance between two corresponding grids, such as Figure 4 shown):

[0064] Calculate the Euclidean distance between the corresponding grid pixels. The grid A0 in image I1 contains pixels (x1, x2, x3, ..., xi), and the grid B0 in image I2 contains pixels (y1, y2, y3, ..., yi). The distance between A0 and B0 is Similarly, calculate the pixel distance values ​​δ1, δ2, ..., δ of all corresponding squares n Compare each distance value with the set threshold λ in turn, and judge the grid with a distance value greater than λ as a dynamic grid.

[0065] Step A24 (selecting the image region of the target feature):

[0066] According to the pixel coordinates obtained by the grid division rule, the largest envelope rectangular area of ​​all dynamic grids is selected, and the origin of the image coordinate system is set at the upper left corner of the image. The x and y squares are vertically downward and horizontally right respectively. Then the coordinate of the upper left vertex of the final dynamic rectangular area S is the minimum value of the coordinates of the upper left vertices of all dynamic grids (x min ,y min ), the lower right vertex coordinate is the maximum value of the lower right vertex coordinates of all dynamic meshes (x max ,y max ).

[0067] Step A3 (obtaining edge features):

[0068] Based on the image area of ​​the target feature, the edge feature residual ε of the reference sensor, the laser radar, and the infrared camera are obtained respectively. e1 , ε e2 , ε e3 , and the corresponding Jacobian matrix L e1 , L e2 , L e3 . For example, by:

[0069] Step A31, determining the edge feature p of the image area of ​​the target feature by an image edge detection algorithm i ;

[0070] Step A32: Determine the edge straight lines of the reference sensor and the additional sensor in the reference sensor coordinate system, and project them to the pixel coordinate system. The edge straight lines projected to the pixel coordinate system are expressed as xcosθ+ysinθ=ρ in the polar coordinate system.

[0071] Step A33, using ε e =ρ-(x e cosθ+y esinθ)(4) to obtain the residual, where x e and y e are the coordinates of the points on the edge line; the corresponding Jacobian matrix is

[0072]

[0073] where λ e =λ ρ +aλ θ ,λ ρ =(Aρcosθ+Bρsinθ+C) / D,λ θ =(Asinθ-Bcosθ) / D, α=x e sinθ-y e cosθ, A, B, C, and D define the 3D plane coordinate system where the detected edge line is located. The plane is defined in the reference sensor coordinate system. Through the above method, the edge feature residual ε of the visible light camera, lidar, and infrared camera are obtained respectively. e1 , ε e2 , ε e3 , and the corresponding Jacobian matrix L e1 , L e2 , L e3 .

[0074] Step A4 (obtaining key point features):

[0075] Key point detection can use the more common Harris detection algorithm, and use the KLT algorithm for key point tracking. Assume that the points on the initial image are The homogeneous coordinates are (x, y, 1), and the corresponding transformation relationship from the target coordinate system to the S1 sensor coordinate system is S1(0) T o . is the homogeneous coordinate of the key point in the current image. The coordinate system transformation relationship when the S1 sensor obtains two different perspectives of the same key point is recorded as S1 T S1(0) Since the key point features are extracted on the plane, the following homography transformation relationship can be obtained: S1 H S1(0) Yes and S1 T S1(0) Related homography transformation matrix. Therefore, the key point feature residual of the two frames can be expressed as:

[0076]

[0077] in q is the transformation relationship from the current target coordinate system to the reference sensor S1 coordinate system. The homography matrix of the corresponding key points on the two frames of images is as follows:

[0078]

[0079] Where n is the normal vector of the plane where the key point is located, and d is the distance from the plane to the origin of the coordinate system of the reference sensor S1 under the perspective of S1(0). S1 R S1(0) and S1 t S1(0) are the rotation and translation matrices of the reference sensor S1 from the S1(0) view to the current view. The corresponding Jacobian matrix is:

[0080]

[0081]

[0082] Through the above method, the key point feature residual ε of the visible light camera and infrared camera are obtained respectively p1 , ε p2 , and the corresponding Jacobian matrix L p1 and L p2 .

[0083] Step A5 (obtaining depth normal features):

[0084] The visible light camera and LiDAR can obtain the depth of the target. Through joint calibration, the target points measured by the LiDAR can be converted to the reference sensor S1 coordinate system and projected onto the S1 image to complete the alignment of the 3D point cloud and the image. For the obtained 3D point cloud data containing depth information, the distance from the point to the surface can be used as the depth normal feature residual:

[0085] ε d =Z(n x x+n y y+n z )+D (10)

[0086] Where Z is the depth value of the point (x, y) in the image coordinate system. x X+n y Y+n z Z+D=0 is the plane equation expression in the reference sensor S1 coordinate system, n x , n y , n z is the current posture of S1. The corresponding Jacobian matrix is:

[0087] L d =[n x n y nz (n z yZ-n y Z) (n x Zn z xZ) (n y xZ-n x yZ)] (11)

[0088] Through the above method, the depth normal feature residual ε of the visible light camera and lidar can be obtained d1 , ε d2 , and the corresponding Jacobian matrix L d1 , L d2 .

[0089] Step A6 (feature fusion):

[0090] Feature fusion is achieved by stacking feature residuals and Jacobian matrices. The visible light camera is selected as the reference sensor S1, and the transformation relationship between the coordinate systems of other sensors and the S1 coordinate system is obtained through joint calibration. S2 T S1 , S3 T S1 Then the speed matrix conversion relationship between each sensor can be obtained. For example, the speed conversion relationship between sensors S1 and S2 can be expressed as:

[0091]

[0092] Where R is the rotation transformation matrix from sensor S1 to S2, and t is the translation transformation matrix from sensor S1 to S2.

[0093] The velocity in the S1 coordinate system can be obtained by the above formula S1 v is expressed in the S2 coordinate system, that is, S2 V S1 S1 v. The velocity matrix of the target tracked by sensor S1 after feature fusion can be obtained by the following formula:

[0094]

[0095] By stacking and fusing different types of features from different sensors and making full use of the data redundancy of the available measurements, the accuracy and robustness of target tracking can be effectively improved.

[0096] Step A7 (attitude solution):

[0097] The posture of the current target coordinate system relative to the reference sensor S1 is obtained by an iterative nonlinear minimization method S1 T o The iterative solution expression is as follows:

[0098] S1(k+1) T o =ΔT -1S1(k) T o (14)

[0099] ΔT= S1(k) T S1(k+1) =exp( S1 v) (15)

[0100] in S1(k) T o is the pose of the target relative to the reference sensor S1 at the kth iteration step, S1(k+1) T o The target's position relative to the reference sensor S1 at the k+1th iteration step, S1(k) T S1(k+1) is the relative pose of S1 in the k+1 iteration step relative to the k iteration step. S1 V is the velocity matrix of the reference sensor S1 obtained in the above feature fusion step, preferably K≥3.

[0101] It should be understood that although this specification is described according to various embodiments, not every embodiment contains only one independent technical solution. This narrative method of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment can also be appropriately combined to form other implementation methods that can be understood by those skilled in the art.

[0102] The above description is only an illustrative embodiment of the present application and is not intended to limit the scope of the present application. Any equivalent changes, modifications and combinations made by any person skilled in the art without departing from the concept and principle of the present application shall fall within the scope of protection of the present application.

Claims

1. A posture estimation method based on multi-sensor multi-feature modular fusion, wherein: The posture estimation method comprises: Step A1: Determine the pixel coordinate system, the intrinsic parameters of the reference sensor, and the extrinsic parameters of the additional sensor, and transform the points in the additional sensor coordinate system to the reference sensor coordinate system, and then project the points in the reference sensor coordinate system to the pixel coordinate system. The reference sensor includes a visible light camera, and the additional sensor includes a lidar and an infrared camera. The extrinsic parameters include the coordinate system transformation matrix of the lidar relative to the reference sensor. S2 T S1 and the coordinate system transformation matrix of the infrared camera relative to the reference sensor S3 T S1 ; Step A2, performing dynamic area filtering on the image in the pixel coordinate system to obtain the image area of ​​the target feature; Step A3: Based on the image area of ​​the target feature, obtain the edge feature residual ε of the reference sensor, the laser radar, and the infrared camera respectively. e1 , ε e2 , ε e3 , and the corresponding Jacobian matrix L e1 , L e2 , L e3 ; Step A4: Based on the image area of ​​the target feature, obtain the key point feature residual ε of the reference sensor and the infrared camera respectively. p1 , ε p2 and the corresponding Jacobian matrix L p1 and L p2 ; Step A5: Based on the image area of ​​the target feature, respectively obtain the depth normal feature residual ε of the reference sensor and the laser radar d1 , ε d2 and the corresponding Jacobian matrix L d1 , L d2 ; Step A6, by S2 T S1 , S3 T S1 Obtain the velocity matrix conversion relationship of the laser radar and the infrared camera relative to the reference sensor respectively S2 V S1 and S3 V S1 , and get the velocity in the S1 coordinate system S1 v, said S1 The expression for v is Step A7, obtain the posture of the current target coordinate system relative to the reference sensor S1 through an iterative nonlinear minimization method S1 T o , the expression to be solved is S1(k+1) T O =ΔT -1S1(k) T O 、 ΔT= S1(k) T S1(k+1) =exp( S1 v), in, S1(k) T o is the pose of the target relative to the reference sensor at the k-th iteration step, S1(k+1) T o is the pose of the target relative to the reference sensor at the k+1th iteration step, S1(k) T S1(k+1) is the relative pose of the reference sensor at the k+1 iteration step relative to the k iteration step.

2. The posture estimation method according to claim 1, wherein The step A2 comprises: Step A21, pre-processing two images I1 and I2 separated by a certain number of frames in time sequence, wherein I1 is the image later in time sequence and I2 is the image earlier in time sequence; Step A22, dividing the I1 and I2 into grid areas respectively; Step A23, calculating the distance between the corresponding grids I1 and I2, and determining the grids with distance values ​​greater than a threshold as dynamic grids; Step A24: For the dynamic grid in I1, the largest envelope rectangular area is selected as the image area of ​​the target feature.

3. The posture estimation method according to claim 2, wherein The preprocessing includes grayscale and Gaussian smoothing.

4. The posture estimation method according to claim 2, wherein Grid area division methods include: The width and height are divided into the same number of grids n, or Keep each grid square with a side length of m pixels.

5. The posture estimation method according to claim 1, wherein The internal parameters include optical characteristic parameters.

6. The posture estimation method according to claim 1, wherein Step A3 includes: Step A31, determining the edge feature p of the image area of ​​the target feature by an image edge detection algorithm i ; Step A32: Determine the edge straight lines of the reference sensor and the additional sensor in the reference sensor coordinate system, and project them to the pixel coordinate system. The edge straight lines projected to the pixel coordinate system are expressed as xcosθ+ysinθ=ρ in the polar coordinate system. Step A33, using ε e =ρ-(x e cosθ+y e sinθ) to get the residual, where x e and y e are the coordinates of the points on the edge line; the corresponding Jacobian matrix is where λ e =λ ρ +αλ θ ,λ ρ =(Aρcosθ+Bρsinθ+C) / D,λ θ =(Asinθ-Bcosθ) / D, α=x e sinθ-y e cosθ, A, B, C, and D define the 3D plane coordinate system where the detected edge line is located. The plane is defined in the reference sensor coordinate system.

7. The posture estimation method according to claim 1, characterized in that The step A4 includes a key point detection process, which includes a Harris detection algorithm and a KLT algorithm for tracking key points.

8. The posture estimation method according to claim 1, wherein The step A5 comprises: Step A51: convert the target point obtained by the laser radar into a reference sensor coordinate system and then project it onto a pixel coordinate system to complete the alignment of the 3D point cloud and the image; Step A52: The distance from the point to the surface is used as the depth normal feature residual ε d1 , ε d2 , and then get the corresponding Jacobian matrix L d1 , L d2 .

9. The posture estimation method according to claim 1, wherein: The additional sensor includes a binocular camera.

10. The posture estimation method according to claim 1, wherein: In step A7, K≥3.

Images