A coordinate system calculation method based on visual images
By using deep learning and adaptive weighted bundle adjustment optimization model, multi-level semantic feature points are extracted and feature relationship graphs are constructed, which solves the problem of coordinate system instability in complex scenarios in traditional methods and achieves high stability and high reliability coordinate system calculation in complex environments.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- 无锡升相科技有限公司
- Filing Date
- 2025-12-09
- Publication Date
- 2026-06-16
Smart Images
Figure BDA0005729076270000032 
Figure BDA0005729076270000041 
Figure BDA0005729076270000071
Abstract
Description
Technical Field
[0001] This invention relates to the field of computer technology, specifically a coordinate system calculation method based on visual images. Background Technology
[0002] Coordinate system calculation based on visual images is one of the core tasks of computer vision, aiming to establish a precise correspondence between the two-dimensional pixel coordinates of an image and the three-dimensional coordinates of the real world. This technology is fundamental to applications such as augmented reality (AR) registration, robot visual servoing, and surgical navigation.
[0003] Traditional methods primarily rely on feature point detection and matching (such as SIFT and ORB) to solve for camera pose using multi-view geometry (such as epipolar geometry and PnP algorithms), thereby deriving coordinate transformations. However, these methods have significant limitations when facing the following complex scenarios:
[0004] 1. Scenes with scarce or repetitive features: In scenes with simple textures, sparse features (such as white walls or smooth workpiece surfaces) or repetitive structures (such as window arrays), traditional manual feature extraction is difficult and has high matching ambiguity, resulting in a large number of initial matching errors and calculation failures.
[0005] 2. Large changes in perspective and scale: When the perspective or shooting distance changes drastically, the invariance of traditional local feature descriptors is limited, the matching accuracy drops sharply, and the stability of coordinate system calculation is seriously affected.
[0006] 3. Dynamic occlusion and lighting changes: Temporary occlusion, motion blur, or drastic lighting changes in the application environment can cause some features to disappear temporarily or their appearance to change. Traditional methods cannot effectively infer the coordinates of the occluded parts. Summary of the Invention
[0007] The technical problem to be solved by this invention is to overcome the above-mentioned technical defects and provide a coordinate system calculation method based on visual images.
[0008] To solve the above problems, the technical solution of the present invention includes the following steps:
[0009] S1. Acquire the original image of the target scene containing at least one known three-dimensional spatial structural feature;
[0010] S2. Preprocess the original image and use a deep learning model to extract multi-level semantic feature points and their local descriptors that correspond to the three-dimensional spatial structure features in the image;
[0011] S3. Based on the extracted multi-level semantic feature points, the prior model of the known three-dimensional spatial structure features is matched with the geometric constraint relationship to solve the initial homography matrix from the image pixel coordinate system to the target scene world coordinate system.
[0012] S4. Construct an adaptive weighted bundle adjustment optimization model. Using the initial homography matrix as the initial value, jointly optimize the reprojection error of the multi-level semantic feature points, the feature point scale consistency error, and the structural constraint error of the prior model, and iteratively solve to obtain the optimal coordinate system transformation matrix.
[0013] S5. Using the optimal coordinate system transformation matrix, map any pixel coordinate in the image to the world coordinate system of the target scene, and output the mapping result and transformation confidence evaluation.
[0014] S6. Timing filtering and drift correction.
[0015] Furthermore, step S2 includes the following steps:
[0016] S2.1. For the preprocessed image, a first convolutional neural network is used to extract geometric feature points and their descriptors. The geometric feature points are corner points, edge intersection points and high curvature points in the image.
[0017] S2.2 For the same image, an instance segmentation neural network is used to identify a specific target object and locate its preset semantic-level feature points, wherein the semantic-level feature points are the physical key points of the target object;
[0018] S2.3. Based on the geometric and semantic feature points and their spatial distribution, construct a feature relationship graph, and generate context-level feature points representing global topological relationships through a graph neural network.
[0019] Furthermore, step S3 includes the following steps:
[0020] S3.1 Match the descriptors of the extracted feature points at each level with the theoretical descriptors of the corresponding three-dimensional points in the prior model to generate an initial set of matching point pairs;
[0021] S3.2. Using a random sampling consensus algorithm combined with the five-point method, the fundamental matrix is robustly estimated from the initial set of matching point pairs. Then, using the absolute scale of the known three-dimensional spatial structural features, the initial homography matrix is decomposed by the perspective n-point algorithm.
[0022] Furthermore, the adaptive weights The calculation method is as follows:
[0023]
[0024] in, Let σ be the descriptor matching similarity score for the i-th feature point, α be the sigmoid function, and α be the score scaling factor. i β is the layer weight coefficient to which the feature point belongs, and β is the layer scaling coefficient. (p i ) represents the magnitude of the image gradient in the neighborhood of the feature point, and δ is a constant excluding zero.
[0025] Furthermore, in step S6, when performing real-time coordinate system calculation on the continuous video stream image, an extended Kalman filter is established with the optimal coordinate system transformation matrix as the state variable; the observed value of the filter is the optimal coordinate system transformation matrix calculated in the current frame, and the process model is based on the assumption of uniform motion or sensor fusion information; through filtering, the coordinate system transformation matrix is temporally smoothed and the accumulated drift error is corrected.
[0026] Furthermore, step S1 includes the following steps:
[0027] S1.1. Use a fixed-focus camera or a zoom camera to acquire one or more frames of the original image, and perform noise reduction, illumination equalization and geometric distortion correction on the original image;
[0028] S1.2 The known three-dimensional spatial structural features are structural information of a target object or environment containing identifiable geometric elements, which is obtained in advance through three-dimensional scanning or CAD modeling.
[0029] Furthermore, the first convolutional neural network for extracting geometric feature point descriptors in step S2.1 is a rotation-invariant dense feature description network. Its network structure includes a feature encoder, a spatial transformation network, and a descriptor generator. The spatial transformation network is used to predict the local orientation of image patches and perform rotation alignment. The descriptor generator outputs a 256-dimensional rotation-invariant descriptor vector.
[0030] Furthermore, the objective function E of the adaptive weighted bundle adjustment optimization model in step S4 is:
[0031]
[0032] Where N is the total number of feature points, p i Let P be the image pixel coordinates of the i-th feature point. i Let π(·) be its corresponding 3D world coordinates, π(·) be the camera projection function, and T be the coordinate system transformation matrix to be optimized. Let s be the adaptive weight for the i-th point. j Let j be the observation scale of the j-th matching point pair in the image. Its theoretical scale in the prior model Let k be the coordinates of the k-th prior constraint point in the current estimated coordinate system. Let λ1 and λ2 be its coordinates in the standard model, ρ be the Huber loss function, and λ1 and λ2 be the balance coefficients.
[0033] Furthermore, the transformation confidence assessment in step S5 specifically includes the calculation and fusion of the following indicators: geometric consistency index, matching reliability index, and matrix stability index. The three indicators are input into a pre-trained three-layer fully connected neural network, and the output of the neural network is an overall confidence score between 0 and 1.
[0034] The advantages of this invention compared to existing technologies are:
[0035] 1. This invention provides a coordinate system calculation method based on visual images, introducing a multi-level semantic feature point extraction mechanism. This mechanism not only extracts geometric features (corners, edges, etc.), but also identifies target objects and locates their semantic feature points (such as physical key points) through an instance segmentation network. Furthermore, it combines a graph neural network to construct context-level feature points to represent global topological relationships. In scenes with simple textures, semantic feature points provide stable and interpretable matching anchors. In repetitive structures, the context graph enhances the discriminative power between features, significantly reduces matching ambiguity, and improves usability and success rate in low-texture, high-repetition scenes.
[0036] 2. This invention provides a coordinate system calculation method based on visual images, employing a rotation-invariant dense feature description network, which includes a spatial transformation network. This network can predict the orientation of local image patches and perform rotational alignment, outputting rotation-invariant descriptors. This ensures that the feature descriptions remain consistent under changes in viewpoint and rotation, improving matching stability under large viewpoint differences. By combining multi-level features with prior models, even with significant scale changes, optimization can still be achieved through scale consistency constraints, enhancing the scale robustness of the system.
[0037] 3. This invention provides a coordinate system calculation method based on visual images. It constructs a feature relationship graph and introduces context-level feature points. It learns the topological dependencies between features through a graph neural network. In the optimization stage, it uses an adaptive weighted bundle adjustment method to dynamically weight the reliability of feature points (such as combining gradient information, matching scores, etc.). When some features are temporarily unreliable due to occlusion or changes in illumination, the system can infer their positions through context relationships and avoid their negative impact on the overall solution by reducing the weight of unreliable features, thereby maintaining stable coordinate estimation capabilities in dynamic environments. Detailed Implementation
[0038] The technical solutions in the embodiments of the present invention will be clearly and completely described below. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0039] This embodiment proposes a coordinate system calculation method based on visual images, including the following steps:
[0040] S1. Acquire the original image of the target scene containing at least one known three-dimensional spatial structural feature;
[0041] S2. Preprocess the original image and use a deep learning model to extract multi-level semantic feature points and their local descriptors that correspond to the three-dimensional spatial structure features in the image;
[0042] S3. Based on the extracted multi-level semantic feature points, the initial homography matrix from the image pixel coordinate system to the target scene world coordinate system is solved by matching the prior model of the known three-dimensional spatial structure features with the geometric constraint relationship.
[0043] S4. Construct an adaptive weighted bundle adjustment optimization model. Using the initial homography matrix as the initial value, jointly optimize the reprojection error of multi-level semantic feature points, the feature point scale consistency error, and the structural constraint error of the prior model. Iteratively solve to obtain the optimal coordinate system transformation matrix.
[0044] S5. Using the optimal coordinate system transformation matrix, map the coordinates of any pixel in the image to the world coordinate system of the target scene, and output the mapping result and transformation confidence evaluation.
[0045] S6. Timing filtering and drift correction.
[0046] Furthermore, step S1 includes the following steps:
[0047] S1.1. Use a fixed-focus camera or a zoom camera to acquire one or more frames of original images, and perform noise reduction, illumination equalization and geometric distortion correction on the original images.
[0048] S1.2 Known three-dimensional spatial structural features are structural information of a target object or environment containing identifiable geometric elements, obtained in advance through three-dimensional scanning or CAD modeling.
[0049] Furthermore, step S2 includes the following steps:
[0050] S2.1. For the preprocessed image, the first convolutional neural network is used to extract geometric feature points and their descriptors. The geometric feature points are the corner points, edge intersection points and high curvature points in the image.
[0051] S2.2 For the same image, an instance segmentation neural network is used to identify specific target objects and locate their preset semantic-level feature points. The semantic-level feature points are the physical key points of the target objects.
[0052] S2.3. Based on geometric and semantic feature points and their spatial distribution, construct a feature relationship graph, and generate context-level feature points that represent global topological relationships through a graph neural network.
[0053] Furthermore, the first convolutional neural network for extracting geometric feature point descriptors in step S2.1 is a rotation-invariant dense feature description network. Its network structure includes a feature encoder, a spatial transformation network, and a descriptor generator. The spatial transformation network is used to predict the local orientation of the image patch and perform rotation alignment. The descriptor generator outputs a 256-dimensional rotation-invariant descriptor vector.
[0054] Furthermore, step S3 includes the following steps:
[0055] S3.1 Match the extracted descriptors of feature points at all levels with the theoretical descriptors of the corresponding 3D points in the prior model to generate an initial set of matching point pairs;
[0056] S3.2. Using the random sampling consensus algorithm combined with the five-point method, the fundamental matrix is robustly estimated from the initial set of matching point pairs. Then, using the absolute scale of the known three-dimensional spatial structural features, the initial homography matrix is decomposed by the perspective n-point algorithm.
[0057] Furthermore, the objective function E of the adaptive weighted bundle adjustment optimization model in step S4 is:
[0058]
[0059] Where N is the total number of feature points, p i Let P be the image pixel coordinates of the i-th feature point. i Let π(·) be its corresponding 3D world coordinates, π(·) be the camera projection function, and T be the coordinate system transformation matrix to be optimized. Let s be the adaptive weight for the i-th point. j Let j be the observation scale of the j-th matching point pair in the image. Its theoretical scale in the prior model Let k be the coordinates of the k-th prior constraint point in the current estimated coordinate system. Let λ1 and λ2 be its coordinates in the standard model, ρ be the Huber loss function, and λ1 and λ2 be the balance coefficients.
[0060] Furthermore, adaptive weights The calculation method is as follows:
[0061]
[0062] in, Let σ be the descriptor matching similarity score for the i-th feature point, α be the sigmoid function, and α be the score scaling factor. i β is the layer weight coefficient to which the feature point belongs, and β is the layer scaling coefficient. Let δ be the magnitude of the image gradient in the neighborhood of the feature point, and let δ be a constant excluding zero.
[0063] Furthermore, the transformation confidence assessment in step S5 specifically includes the calculation and fusion of the following indicators: geometric consistency index, matching reliability index, and matrix stability index. The three indicators are input into a pre-trained three-layer fully connected neural network, and the output of the neural network is an overall confidence score between 0 and 1.
[0064] Furthermore, in step S6, when performing real-time coordinate system calculation on the continuous video stream image, an extended Kalman filter is established with the optimal coordinate system transformation matrix as the state variable; the observed value of the filter is the optimal coordinate system transformation matrix calculated in the current frame, and the process model is based on the assumption of uniform motion or sensor fusion information; through filtering, the coordinate system transformation matrix is smoothed in time and the accumulated drift error is corrected.
[0065] In practical use, image acquisition and preprocessing are performed using a Basler acA2500-14gm industrial camera (2560×1440 resolution, 30fps frame rate, 8mm focal length) to acquire raw images of the casing from multiple angles. Preprocessing workflow:
[0066] ①5×5 Gaussian filter noise reduction (reduces sensor noise, improves signal-to-noise ratio from 28dB to 42dB);
[0067] ②CLAHE algorithm lighting equalization (suppresses fluctuations in workshop lighting brightness, reducing the standard deviation of grayscale values from 45 to 12);
[0068] ③ Radial distortion correction based on calibration parameters (eliminating lens distortion, distortion error ≤ 0.05 pixels).
[0069] Known 3D feature acquisition: The shell is scanned using a Faro Focus S70 3D laser scanner (accuracy ±0.02mm), and the centers (φ10mm) of the three positioning reference holes and two corner points of the shell edge are extracted as known 3D structural features to generate a priori model with world coordinates (e.g., coordinates of reference hole A (120.0, 85.0, 0.0)mm, coordinates of reference hole B (220.0, 85.0, 0.0)mm).
[0070] Geometric feature point extraction: D2-Net (Rotation Invariant Dense Feature Description Network) is used as the first convolutional neural network, inputting the preprocessed image. Network configuration: The feature encoder is a 6-layer convolutional layer (3×3 kernel, stride 1, padding = 1), the spatial transformation network predicts the local orientation of 3×3 image patches and rotates and aligns them (angle error ≤ 2°), and the descriptor generator outputs a 256-dimensional floating-point descriptor vector. Finally, 1200 geometric feature points, such as corner points and edge intersections, are extracted, achieving a feature point density of 6 per square millimeter (5 times higher than traditional ORB).
[0071] Semantic-level feature point localization: The Mask R-CNN instance segmentation network (pre-trained weights fine-tuned based on the COCO dataset) is used to identify the shell region and locate the preset semantic-level feature points - the center of 3 reference holes and 2 corner points, and output their pixel coordinates (e.g., the pixel coordinates of reference hole A (1024,768)). These feature points are not affected by surface smoothness and the localization repeatability is ≤0.3 pixels.
[0072] Context-level feature point generation: A feature relationship graph is constructed using geometric and semantic feature points as nodes and Euclidean distances less than 50 pixels as edges. The input graph is a convolutional neural network (GCN, 2 hidden layers, ReLU activation function). By learning topological relationships such as the horizontal distance between reference hole A and reference hole B and the vertical offset between the corner point and the reference hole, 800 context-level feature points are generated, enhancing the feature discrimination of repetitive structural regions.
[0073] Feature matching: The descriptors of the three-level feature points (256 dimensions of geometric level + 128 dimensions of semantic level + 64 dimensions of context level, concatenated to 448 dimensions) are matched with the theoretical descriptors of the corresponding three-dimensional points in the prior model (generated by the same network) using Euclidean distance. The matching distance threshold is set to 0.7, generating 1560 initial matching point pairs.
[0074] Matrix solution: The RANSAC algorithm was used to remove 240 outlier matching pairs (inlier rate 84.6%), and the five-point method was used to estimate the fundamental matrix (reprojection error 2.6 pixels). The actual distance of 100mm between the reference holes A and B in the prior model was used as the absolute scale, and the fundamental matrix was decomposed by the EPnP (efficient perspective n-point) algorithm to obtain the initial homography matrix H0.
[0075] Objective function construction: Determine the parameter configuration of the objective function E—λ1 = 1.0 (scale consistency error weight), λ2 = 1.8 (structural constraint error weight), and the inflection point of the Huber loss function ρ is 1.0 (suppressing the influence of outliers).
[0076] Taking a semantic-level feature point (reference aperture A) as an example, its S i^match = 0.92 (match score), α = 6 (score scaling factor), L i =1.0 (semantic weight coefficient), β = 2.5 (hierarchical scaling coefficient), (Gradient magnitude), δ=1e-6 (excluding zero constant).
[0077] Using H0 as the initial value, the coordinate data of 1320 interior feature points are substituted, and the Levenberg-Marquardt algorithm is iterated for 6 times to converge, obtaining the optimal coordinate system transformation matrix T*, which reduces the reprojection error from 2.6 pixels to 1.1 pixels.
[0078] Using T*, any pixel on the shell gripping surface (e.g., (1200, 800)) is mapped to world coordinates, and the result is (185.3, 102.6, 5.2) mm. The deviation from the laser measurement value (185.5, 102.7, 5.1) mm is ≤0.2 mm.
[0079] ① Geometric consistency index M_reproj = 1.0 pixels (median reprojection error);
[0080] ② The matching reliability index R_inlier = 84.6% (inside point ratio);
[0081] ③ The matrix stability index C_cond = 120 (the condition number of T*, <300 indicates stability). The three indices are input into a pre-trained three-layer fully connected neural network (input dimension 3, hidden layer 64 dimensions, output dimension 1), and the overall confidence score is 0.93 (≥0.8 indicates high reliability).
[0082] An extended Kalman filter (EKF) was established: the state vector x contains 6 degrees of freedom (3 translations + 3 rotations) of T*; the process model is based on the assumption of uniform robot motion (grasping speed 0.1 m / s), and the process noise covariance Q = diag([1e-6, 1e-6, 1e-6, 1e-8, 1e-8, 1e-8]); the observation noise covariance R is dynamically adjusted according to the confidence level (R = 0.01 × I at a confidence level of 0.93). The coordinate drift of 100 consecutive frames decreased from 2.3 mm without filtering to 0.4 mm.
[0083] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus.
[0084] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.
[0085] The present invention and its embodiments have been described above. This description is not restrictive, and the actual structure is not limited thereto. In conclusion, if those skilled in the art are inspired by this description and, without departing from the spirit of the invention, design similar structures and embodiments without creative effort, all such designs should fall within the protection scope of the present invention.
Claims
1. A coordinate system calculation method based on visual images, characterized in that, Includes the following steps: S1. Acquire the original image of the target scene containing at least one known three-dimensional spatial structural feature; S2. Preprocess the original image and use a deep learning model to extract multi-level semantic feature points and their local descriptors that correspond to the three-dimensional spatial structure features in the image; S3. Based on the extracted multi-level semantic feature points, the prior model of the known three-dimensional spatial structure features is matched with the geometric constraint relationship to solve the initial homography matrix from the image pixel coordinate system to the target scene world coordinate system. S4. Construct an adaptive weighted bundle adjustment optimization model. Using the initial homography matrix as the initial value, jointly optimize the reprojection error of the multi-level semantic feature points, the feature point scale consistency error, and the structural constraint error of the prior model, and iteratively solve to obtain the optimal coordinate system transformation matrix. S5. Using the optimal coordinate system transformation matrix, map any pixel coordinate in the image to the world coordinate system of the target scene, and output the mapping result and transformation confidence evaluation. S6. Timing filtering and drift correction.
2. The coordinate system calculation method based on visual images according to claim 1, characterized in that, Step S1 includes the following steps: S1.
1. Use a fixed-focus camera or a zoom camera to acquire one or more frames of the original image, and perform noise reduction, illumination equalization and geometric distortion correction on the original image; S1.2 The known three-dimensional spatial structural features are structural information of a target object or environment containing identifiable geometric elements, which is obtained in advance through three-dimensional scanning or CAD modeling.
3. The coordinate system calculation method based on visual images according to claim 2, characterized in that, Step S2 includes the following steps: S2.
1. For the preprocessed image, a first convolutional neural network is used to extract geometric feature points and their descriptors. The geometric feature points are corner points, edge intersection points and high curvature points in the image. S2.2 For the same image, an instance segmentation neural network is used to identify a specific target object and locate its preset semantic-level feature points, wherein the semantic-level feature points are the physical key points of the target object; S2.
3. Based on the geometric and semantic feature points and their spatial distribution, construct a feature relationship graph, and generate context-level feature points representing global topological relationships through a graph neural network.
4. The coordinate system calculation method based on visual images according to claim 3, characterized in that, The first convolutional neural network for extracting geometric feature point descriptors in step S2.1 is a rotation-invariant dense feature description network. Its network structure includes a feature encoder, a spatial transformation network, and a descriptor generator. The spatial transformation network is used to predict the local orientation of image patches and perform rotation alignment. The descriptor generator outputs a 256-dimensional rotation-invariant descriptor vector.
5. The coordinate system calculation method based on visual images according to claim 3, characterized in that, Step S3 includes the following steps: S3.1 Match the descriptors of the extracted feature points at each level with the theoretical descriptors of the corresponding three-dimensional points in the prior model to generate an initial set of matching point pairs; S3.
2. Using a random sampling consensus algorithm combined with the five-point method, the fundamental matrix is robustly estimated from the initial set of matching point pairs. Then, using the absolute scale of the known three-dimensional spatial structural features, the initial homography matrix is decomposed by the perspective n-point algorithm.
6. The coordinate system calculation method based on visual images according to claim 1, characterized in that, The objective function E of the adaptive weighted bundle adjustment optimization model in step S4 is: Where N is the total number of feature points, p i Let P be the image pixel coordinates of the i-th feature point. i Let π(·) be its corresponding 3D world coordinates, π(·) be the camera projection function, and T be the coordinate system transformation matrix to be optimized. Let s be the adaptive weight for the i-th point. j Let j be the observation scale of the j-th matching point pair in the image. Its theoretical scale in the prior model Let k be the coordinates of the k-th prior constraint point in the current estimated coordinate system. Let λ1 and λ2 be its coordinates in the standard model, ρ be the Huber loss function, and λ1 and λ2 be the balance coefficients.
7. The coordinate system calculation method based on visual images according to claim 6, characterized in that, The adaptive weight The calculation method is as follows: in, Let σ be the descriptor matching similarity score for the i-th feature point, α be the sigmoid function, and α be the score scaling factor. i β is the layer weight coefficient to which the feature point belongs, and β is the layer scaling coefficient. Let δ be the magnitude of the image gradient in the neighborhood of the feature point, and δ be a constant excluding zero.
8. The coordinate system calculation method based on visual images according to claim 1, characterized in that, The transformation confidence assessment in step S5 specifically includes the calculation and fusion of the following indicators: geometric consistency index, matching reliability index, and matrix stability index. The three indicators are input into a pre-trained three-layer fully connected neural network, and the output of the neural network is an overall confidence score between 0 and 1.
9. The coordinate system calculation method based on visual images according to claim 1, characterized in that, In step S6, when performing real-time coordinate system calculation on continuous video stream images, an extended Kalman filter is established with the optimal coordinate system transformation matrix as the state variable. The filter's observation value is the optimal coordinate system transformation matrix calculated for the current frame. The process model is based on the assumption of uniform motion or sensor fusion information. Through filtering, the coordinate system transformation matrix is smoothed in time and the accumulated drift error is corrected.