Linear rigid body motion tracking system and method based on super-resolution

Abstract

The invention discloses a linear rigid body motion tracking system and method based on super-resolution. The method comprises the following steps: performing super-resolution on video frames acquired through video stream data; obtaining feature information of a rigid body in the super-resolution image; and performing real-time detection and three-dimensional space motion tracking on each rigid body. According to the invention, linear motion tracking can be carried out on the to-be-tracked rigid body in the monitoring video, and high tracking accuracy in an industrial environment is ensured.

Description

Technical Field

[0001] The present invention belongs to the technical field of defect detection, and in particular relates to a linear rigid body motion tracking system and method based on super-resolution. Background Art

[0002] In the modern power grid industry, video surveillance technology is widely used in key links such as equipment inspection, fault detection and safety management. However, due to historical reasons and technical limitations, many power grid monitoring systems still use low-resolution cameras, which results in poor video quality and difficulty in accurately identifying and monitoring key equipment status and operating conditions. With the increasing complexity of power grid equipment and operating environment, traditional low-resolution videos can no longer meet the current power grid industry's demand for high-precision and high-efficiency inspections. Therefore, improving video resolution and enhancing video analysis capabilities have become important directions for technology upgrades in the power grid industry.

[0003] Super-resolution video technology reconstructs low-resolution videos into high-resolution videos by using artificial intelligence and deep learning algorithms, thereby significantly improving the clarity and detail of the videos. The application of this technology in the power grid industry, especially for upgrading the video quality of old videos and low-performance terminals, has important practical significance. By building a deep neural network, super-resolution reconstruction of old surveillance videos can be achieved, which can not only enhance the visualization of the video, but also improve the target recognition and fault detection accuracy in power grid equipment inspections. The application of this technology will greatly improve the efficiency and reliability of power grid inspections and help ensure the safe operation of the power system.

[0004] During the power grid inspection process, accurate identification and tracking of mobile targets is crucial to ensure the safety of power facilities. In traditional power grid inspection scenarios, the monitoring of mobile targets often relies on manual observation and simple monitoring equipment, which is not only time-consuming and labor-intensive, but also easily affected by human factors, resulting in missed detection or false detection. Mobile target tracking technology based on frame difference method and optical flow method can achieve fast real-time monitoring and position tracking of mobile targets by analyzing the continuity of video sequences.

[0005] Based on the above considerations, it is necessary to propose a linear rigid body motion tracking system and method based on super-resolution. By combining super-resolution video technology with mobile target tracking technology based on frame difference method / optical flow method, the power grid industry will be able to better cope with the increasingly complex inspection needs and ensure the safe and stable operation of power facilities. The research and application of these technologies not only promotes the upgrading of power grid monitoring systems, but also lays a solid foundation for the construction of future smart grids. Summary of the invention

[0006] In view of the above-mentioned deficiencies in the prior art, an object of the present invention is to provide a linear rigid body motion tracking system and method based on super-resolution, so as to solve the problem in the prior art that old low-resolution camera equipment cannot be used to accurately identify and monitor the equipment status and operating conditions.

[0007] To achieve the above object, the technical solution adopted by the present invention is as follows:

[0008] A linear rigid body motion tracking system based on super-resolution of the present invention comprises: an image super-resolution module, a detection module and a tracking module;

[0009] An image super-resolution module is used to perform super-resolution on video frames obtained through video stream data;

[0010] A detection module is used to obtain feature information of rigid bodies in super-resolution images;

[0011] Tracking module is used to perform real-time detection and three-dimensional spatial motion tracking of each rigid body.

[0012] The present invention also provides a linear rigid body motion tracking method based on super-resolution, based on the above system, the steps are as follows:

[0013] (1) Super-resolution the video frames obtained through the video stream data;

[0014] (2) Obtaining feature information of rigid bodies in super-resolution images;

[0015] (3) Perform real-time detection and three-dimensional spatial motion tracking for each rigid body.

[0016] Furthermore, the step (1) specifically includes:

[0017] (11) Receive the video stream data collected by the camera and obtain a video frame I with a size of H×W LR , where H and W represent the height and width of the image respectively;

[0018] (12) Use several convolutional layers to extract features from low-resolution images, including:

[0019] The first convolutional layer uses F1 convolution kernels of size K1×K1 to perform convolution on video frame I LR Perform convolution operation to generate the first layer feature map; then extract higher-level features by stacking several convolution layers, each convolution layer is equipped with a nonlinear activation function; after calculation, the size of the feature extraction is v 2 ×H×W video frame;

[0020] (13) For the above dimension v 2×H×W video frames are rearranged to obtain a vH×vW high-resolution image I HR ,as follows:

[0021]

[0022] Among them, I HR (x, y) is the pixel value of the high-resolution image at position (x, y), Y(i, j, k) is the pixel value of the low-resolution feature map at position (i, j) and channel k, v is the magnification factor, and It means that the x and y coordinates are divided by the magnification factor v and then rounded down. mod is the modulus operation.

[0023] Furthermore, the step (2) includes the following steps: calculating inter-frame differences, extracting feature points, tracking feature points, and calculating rotation pivot points;

[0024] Calculate the difference between frames: Use the frame difference method to determine the region of interest (ROI) of the rigid body;

[0025] Feature point extraction: extract and divide feature points of the area of ​​interest;

[0026] Feature point tracking: Use optical flow method to track feature points;

[0027] Rotation pivot calculation: Reduce the rotation pivot position of each rigid body.

[0028] Furthermore, the step (2) specifically includes:

[0029] (21) Using the frame difference method for two adjacent frames, the motion information of the rigid body is extracted from the rigid body motion video stream; by dilating and binarizing the result D of the frame difference method, a motion region that may contain the rigid body is obtained, which is used as the region of interest to extract the feature points of the rigid body;

[0030] (22) Through the feature point set division algorithm, the feature points of the area of ​​interest in the image are divided into N feature point sets, and the number of rigid bodies N in the image and their corresponding feature point sets are obtained.

[0031] (23) In the subsequent S frames, the optical flow method is used to analyze the feature point set Tracking is as follows:

[0032] I x (x,y)u(x,y)+I y (x,y)v(x,y)+I t (x,y)=0

[0033] Among them, I x (x,y),Iy (x,y),I t (x, y) represents the partial derivatives of the gray value of the feature point (x, y) on the X-axis, Y-axis and time t respectively, u(x, y), v(x, y) represent the velocity vector of the feature point (x, y) on the X-axis and Y-axis respectively, then solve (u, v), and use the 9 pixels in the 3×3 window centered on the feature point to establish the equation as follows:

[0034]

[0035] Use the least squares method to find the optimal (u, v), and according to the frame interval dt, get the position (x0, y0) of the feature point (x, y) in the current frame, as follows:

[0036] x0=x+udt

[0037] y0=y+vdt

[0038] Get the feature point set of the rigid body in the current frame s

[0039] (24) The feature point sequence of the rigid body i is obtained by extracting the feature points of one frame and tracking the optical feature points of the subsequent S frames through the frame difference method and the optical flow method. The feature point set of each frame is calculated by the least squares method. Fitting to obtain a straight line As a result of rigid body detection, the calculation method of line parameters A, B, C is as follows:

[0040]

[0041] Among them, (x i ,y i ) is the feature point set The point in

[0042] Through the feature point set sequence, the corresponding straight line sequence is obtained Combine S+1 straight lines and use the least squares method to get the point (x, y) with the shortest distance from all the straight lines as the rotation fulcrum K of the rigid body. i , (x,y) is calculated as follows:

[0043]

[0044] The value range of S is S≥2, which keeps the result of the rotation support calculation stable.

[0045] Further, the step (3) includes the following steps: generating a region of interest, detecting a rigid body, calculating a reference length, and tracking motion in three-dimensional space;

[0046] Region of interest generation: determine the area where the rigid body is more likely to appear in the current frame;

[0047] Rigid body detection: extract the rigid body and select the reference frame of the rigid body;

[0048] Reference length calculation: calculate the reference length when the rigid body is parallel to the camera plane;

[0049] Three-dimensional space motion tracking: tracking the motion of rigid bodies in three-dimensional space.

[0050] Furthermore, the step (3) specifically includes:

[0051] (31) According to the rotation fulcrum of the rigid body and the detection result of the previous frame, the possible appearance area of ​​the rigid body in the current frame is determined as the detection area of ​​interest, and the plane angle calculation result α of the previous two frames is taken -2 and α -1 , determine the deflection angle of the rigid body between the two frames, which is the motion angular velocity vector V containing the motion direction information, as follows:

[0052] V=α -1 -α -2

[0053] Thus, the specific angle range of the region of interest (ROI) is obtained and refined. For the direction of motion of the rigid body, the rotation angle of the rigid body in the previous frame is used as the starting point to detect the angle of the motion direction. The angle range of the detection depends on the angular velocity V of the rigid body in the previous frame. A small range of inspection is also performed in the opposite direction to prevent the rigid body's motion direction from changing suddenly. If the current rigid body is stationary, the same range of detection is performed on both clockwise and counterclockwise directions:

[0054]

[0055] Among them, k,θ are hyperparameters used to cope with the change of rigid body angular velocity, ∈ is the minimum unit of angle change during line detection, which is used to cope with the situation where the rigid body's motion direction changes suddenly;

[0056] (32) According to the rotation fulcrum of the rigid body and the region of interest, the rigid body is detected from the region of interest by straight line detection with a known endpoint. At the same time, the motion information of the rigid body is combined to eliminate background interference and the straight line detection result L is calculated. i The frame difference result D of two adjacent frames i Take the intersection and use the motion information of the rigid body to obtain a more robust detection result, as follows:

[0057] L i =L i ∩D i

[0058] Through the above straight line detection process, the angle between the corresponding straight line of the rigid body and the horizontal plane is the plane rotation angle α of the rigid body, and the length of the straight line is the length S of the rigid body. k ;

[0059] (33) The rigid body is selected as the reference frame when it is stationary. The inertial sensor is used to obtain the angle β′ with the camera plane. The magnetometer is used to select an initial vector v0 in the three-dimensional space. Then, the rotation matrix A1 of v0 to the same direction as the rigid body in the reference frame and the rotation matrix A2 to the same direction as the camera are obtained. The three-dimensional vectors v1 and v2 representing the directions of the rigid body and the camera in the three-dimensional space are obtained using the rotation matrix as follows:

[0060] v i =A i v0,i∈[1,2]

[0061] According to the three-dimensional vectors of the rigid body and the camera, the angle β′ between the rigid body and the camera plane is obtained as follows:

[0062]

[0063] According to the rigid body length S in the reference frame k Calculate the reference length S′ i ,as follows:

[0064]

[0065] (34) According to the length S of rigid body i in the current frame i and its reference length S′ i The vertical angle β is calculated as follows:

[0066]

[0067] Thus, the three-dimensional spatial motion tracking of the rigid body is completed, and the plane rotation angle α and the vertical rotation angle β of the rigid body are obtained.

[0068] Beneficial effects of the present invention:

[0069] 1. The present invention can perform linear motion tracking of the rigid body to be tracked in the surveillance video, ensuring high tracking accuracy in industrial environments;

[0070] 2. The present invention utilizes the characteristics of linear rigid body rotational motion, utilizes two-stage detection and tracking, and detects the motion of the target object to distinguish the target object from the background, thereby solving the problem of difficulty in distinguishing the target object from the background;

[0071] 3. The present invention optimizes the tracking phase, which occupies most of the system operation time, by refining the region of interest of the motion direction and motion speed, thereby solving the problem that as the number of target objects increases, the computational complexity increases, making it difficult for lightweight devices to meet real-time requirements. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] Figure 1 is a flow chart of the method of the present invention;

[0073] Figure 2 Schematic diagram of video frame super-resolution;

[0074] Figure 3 Refine the schematic diagram for the region of interest;

[0075] Figure 4 This is a schematic diagram of the benchmark length calculation. DETAILED DESCRIPTION

[0076] In order to facilitate the understanding of those skilled in the art, the present invention is further described below in conjunction with embodiments and drawings. The contents mentioned in the implementation modes are not intended to limit the present invention.

[0077] A linear rigid body motion tracking system based on super-resolution of the present invention comprises: an image super-resolution module, a detection module and a tracking module;

[0078] The image super-resolution module performs super-resolution on the video frames obtained through the video stream data;

[0079] A detection module is used to obtain feature information of rigid bodies in super-resolution images;

[0080] Tracking module is used to perform real-time detection and three-dimensional spatial motion tracking of each rigid body.

[0081] Reference Figure 1-Figure 4 As shown, the present invention also provides a linear rigid body motion tracking method based on super-resolution, the steps are as follows:

[0082] (1) Super-resolution of video frames obtained through video stream data; specifically including:

[0083] (11) Receive the video stream data collected by the camera and obtain a video frame I with a size of H×W LR , where H and W represent the height and width of the image respectively;

[0084] (12) Use several convolutional layers to extract features from low-resolution images, including:

[0085] The first convolutional layer uses F1 convolution kernels of size K1×K1 to perform convolution on video frame I LRPerform convolution operation to generate the first layer feature map; then extract higher-level features by stacking several convolution layers, each convolution layer is equipped with a nonlinear activation function; after calculation, the size of the feature extraction is v 2 ×H×W video frame;

[0086] (13) For the above dimension v 2 ×H×W video frames are rearranged to obtain a vH×vW high-resolution image I HR ,as follows:

[0087]

[0088] Among them, I HR (x, y) is the pixel value of the high-resolution image at position (x, y), Y(i, j, k) is the pixel value of the low-resolution feature map at position (i, j) and channel k, v is the magnification factor, and It means that the x and y coordinates are divided by the magnification factor v and then rounded down. mod is the modulus operation.

[0089] (2) Obtaining feature information of the rigid body in the super-resolution image; including the following steps: calculating inter-frame differences, extracting feature points, tracking feature points, and calculating rotation pivot points;

[0090] Calculate the difference between frames: Use the frame difference method to determine the region of interest (ROI) of the rigid body;

[0091] Feature point extraction: extract and divide feature points of the area of ​​interest;

[0092] Feature point tracking: Use optical flow method to track feature points;

[0093] Rotation pivot calculation: Reduce the rotation pivot position of each rigid body.

[0094] Specifically, the step (2) comprises:

[0095] (21) Using the frame difference method for two adjacent frames, the motion information of the rigid body is extracted from the rigid body motion video stream; by dilating and binarizing the result D of the frame difference method, a motion region that may contain the rigid body is obtained, which is used as the region of interest to extract the feature points of the rigid body;

[0096] (22) Through the feature point set division algorithm, the feature points of the area of ​​interest in the image are divided into N feature point sets, and the number of rigid bodies N in the image and their corresponding feature point sets are obtained.

[0097] (23) In the subsequent S frames, the optical flow method is used to analyze the feature point set Tracking is as follows:

[0098] I x (x,y)u(x,y)+I y (x,y)v(x,y)+I t (x,y)=0

[0099] Among them, I x (x,y),I y (x,y),I t (x, y) represents the partial derivatives of the gray value of the feature point (x, y) on the X-axis, Y-axis and time t respectively, u(x, y), v(x, y) represent the velocity vector of the feature point (x, y) on the X-axis and Y-axis respectively, then solve (u, v), and use the 9 pixels in the 3×3 window centered on the feature point to establish the equation as follows:

[0100]

[0101] Use the least squares method to find the optimal (u, v), and according to the frame interval dt, get the position (x0, y0) of the feature point (x, y) in the current frame, as follows:

[0102] x0=x+udt

[0103] y0=y+vdt

[0104] Get the feature point set of the rigid body in the current frame s

[0105] (24) The feature point sequence of the rigid body i is obtained by extracting the feature points of one frame and tracking the optical feature points of the subsequent S frames through the frame difference method and the optical flow method. The feature point set of each frame is calculated by the least squares method. Fitting to obtain a straight line As a result of rigid body detection, the calculation method of line parameters A, B, C is as follows:

[0106]

[0107] Among them, (x i ,y i ) is the feature point set The point in

[0108] Through the feature point set sequence, the corresponding straight line sequence is obtained Combine S+1 straight lines and use the least squares method to get the point (x, y) with the shortest distance from all the straight lines as the rotation fulcrum K of the rigid body. i , (x,y) is calculated as follows:

[0109]

[0110] The value range of S is S≥2, which keeps the result of the rotation support calculation stable.

[0111] (3) Real-time detection and three-dimensional motion tracking of each rigid body;

[0112] Region of interest generation, rigid body detection, reference length calculation, and 3D motion tracking;

[0113] Region of interest generation: determine the area where the rigid body is more likely to appear in the current frame;

[0114] Rigid body detection: extract the rigid body and select the reference frame of the rigid body;

[0115] Reference length calculation: calculate the reference length when the rigid body is parallel to the camera plane;

[0116] Three-dimensional space motion tracking: tracking the motion of rigid bodies in three-dimensional space.

[0117] Wherein, the step (3) specifically includes:

[0118] (31) According to the rotation fulcrum of the rigid body and the detection result of the previous frame, the possible appearance area of ​​the rigid body in the current frame is determined as the detection area of ​​interest, and the plane angle calculation result α of the previous two frames is taken -2 and α -1 , determine the deflection angle of the rigid body between the two frames, which is the motion angular velocity vector V containing the motion direction information, as follows:

[0119] V=α -1 -α -2

[0120] Thus, the specific angle range of the region of interest (ROI) is obtained and refined. For the direction of motion of the rigid body, the rotation angle of the rigid body in the previous frame is used as the starting point to detect the angle of the motion direction. The angle range of the detection depends on the angular velocity V of the rigid body in the previous frame. A small range of inspection is also performed in the opposite direction to prevent the rigid body's motion direction from changing suddenly. If the current rigid body is stationary, the same range of detection is performed on both clockwise and counterclockwise directions:

[0121]

[0122] Among them, k,θ are hyperparameters used to cope with the change of rigid body angular velocity, ∈ is the minimum unit of angle change during line detection, which is used to cope with the situation where the rigid body's motion direction changes suddenly;

[0123] (32) According to the rotation fulcrum of the rigid body and the region of interest, the rigid body is detected from the region of interest by straight line detection with a known endpoint. At the same time, the motion information of the rigid body is combined to eliminate background interference and the straight line detection result L is calculated. iThe frame difference result D of two adjacent frames i Take the intersection and use the motion information of the rigid body to obtain a more robust detection result, as follows:

[0124] L i =L i ∩D i

[0125] Through the above straight line detection process, the angle between the corresponding straight line of the rigid body and the horizontal plane is the plane rotation angle α of the rigid body, and the length of the straight line is the length S of the rigid body. k ;

[0126] (33) The rigid body is selected as the reference frame when it is stationary. The inertial sensor is used to obtain the angle β′ with the camera plane. The magnetometer is used to select an initial vector v0 in the three-dimensional space. Then, the rotation matrix A1 of v0 to the same direction as the rigid body in the reference frame and the rotation matrix A2 to the same direction as the camera are obtained. The three-dimensional vectors v1 and v2 representing the directions of the rigid body and the camera in the three-dimensional space are obtained using the rotation matrix as follows:

[0127] v i =A i v0,i∈[1,2]

[0128] According to the three-dimensional vectors of the rigid body and the camera, the angle β′ between the rigid body and the camera plane is obtained as follows:

[0129]

[0130] According to the rigid body length S in the reference frame k Calculate the reference length S′ i ,as follows:

[0131]

[0132] (34) According to the length S of rigid body i in the current frame i and its reference length S′ i The vertical angle β is calculated as follows:

[0133]

[0134] Thus, the three-dimensional spatial motion tracking of the rigid body is completed, and the plane rotation angle α and the vertical rotation angle β of the rigid body are obtained.

[0135] The present invention has many specific application paths. The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements can be made without departing from the principle of the present invention. These improvements should also be regarded as the protection scope of the present invention.

Claims

1. A linear rigid body motion tracking system based on super-resolution, characterized in that: include: Image super-resolution module, detection module and tracking module; An image super-resolution module is used to perform super-resolution on video frames obtained through video stream data; A detection module is used to obtain feature information of rigid bodies in super-resolution images; Tracking module is used to perform real-time detection and three-dimensional spatial motion tracking of each rigid body.

2. A linear rigid body motion tracking method based on super-resolution, based on the system of claim 1, characterized in that: Here are the steps: (1) Super-resolution the video frames obtained through the video stream data; (2) Obtaining feature information of rigid bodies in super-resolution images; (3) Perform real-time detection and three-dimensional spatial motion tracking for each rigid body.

3. The linear rigid body motion tracking method based on super-resolution according to claim 2, characterized in that: The step (1) specifically comprises: (11) Receive the video stream data collected by the camera and obtain a video frame I with a size of H×W LR , where H and W represent the height and width of the image respectively; (12) Use several convolutional layers to extract features from low-resolution images, including: The first convolutional layer uses F1 convolution kernels of size K1×K1 to perform convolution on video frame I LR Perform convolution operation to generate the first layer feature map; then extract higher-level features by stacking several convolution layers, each convolution layer is equipped with a nonlinear activation function; after calculation, the size of the feature extraction is v 2 ×H×W video frame; (13) For the above dimension v 2 ×H×W video frames are rearranged to obtain a vH×vW high-resolution image I HR ,as follows: Among them, I HR (x, y) is the pixel value of the high-resolution image at position (x, y), Y(i, j, k) is the pixel value of the low-resolution feature map at position (i, j) and channel k, v is the magnification factor, and It means that the x and y coordinates are divided by the magnification factor v and then rounded down. mod is the modulus operation.

4. The linear rigid body motion tracking method based on super-resolution according to claim 2, characterized in that: The step (2) includes the following steps: calculating inter-frame differences, extracting feature points, tracking feature points, and calculating rotation pivot points; Calculate the difference between frames: Use the frame difference method to determine the region of interest of the rigid body; Feature point extraction: extract and divide feature points of the area of ​​interest; Feature point tracking: Use optical flow method to track feature points; Rotation pivot calculation: Reduce the rotation pivot position of each rigid body.

5. The linear rigid body motion tracking method based on super-resolution according to claim 4, characterized in that: The step (2) specifically comprises: (21) Using the frame difference method for two adjacent frames, the motion information of the rigid body is extracted from the rigid body motion video stream; by dilating and binarizing the result D of the frame difference method, a motion region that may contain the rigid body is obtained, which is used as the region of interest to extract the feature points of the rigid body; (22) Through the feature point set division algorithm, the feature points of the area of ​​interest in the image are divided into N feature point sets, and the number of rigid bodies N in the image and their corresponding feature point sets are obtained. (23) In the subsequent S frames, the optical flow method is used to analyze the feature point set Tracking is as follows: I x (x,y)u(x,y)+I y (x,y)v(x,y)+I t (x,y)=0 Among them, I x (x,y),I y (x,y),I t (x, y) represents the partial derivatives of the gray value of the feature point (x, y) on the X-axis, Y-axis and time t, u(x, y) and v(x, y) represent the velocity vectors of the feature point (x, y) on the X-axis and Y-axis, respectively. To solve (u, v), the equation is established using the 9 pixels in the 3×3 window centered on the feature point, as follows: Use the least squares method to find the optimal (u,u), and according to the frame interval dt, get the position (x0,y0) of the feature point (x,y) in the current frame, as follows: x0=x+udt y0=y+vdt Get the feature point set of the rigid body in the current frame s (24) The feature point sequence of the rigid body i is obtained by extracting the feature points of one frame and tracking the optical feature points of the subsequent S frames through the frame difference method and the optical flow method. The feature point set of each frame is calculated by the least squares method. Fitting to obtain a straight line As a result of rigid body detection, the calculation method of line parameters A, B, C is as follows: Among them, (x i ,y i ) is the feature point set The point in Through the feature point set sequence, the corresponding straight line sequence is obtained Combine S+1 straight lines and use the least squares method to get the point (x, y) with the shortest distance from all the straight lines as the rotation fulcrum K of the rigid body. i , (x,y) is calculated as follows: Among them, the value range of S is S≥2.

6. The linear rigid body motion tracking method based on super-resolution according to claim 2, characterized in that: The step (3) includes the following steps: generating a region of interest, detecting a rigid body, calculating a reference length, and tracking a three-dimensional motion; Region of interest generation: determine the area where the rigid body is more likely to appear in the current frame; Rigid body detection: extract the rigid body and select the reference frame of the rigid body; Reference length calculation: calculate the reference length when the rigid body is parallel to the camera plane; Three-dimensional space motion tracking: tracking the motion of rigid bodies in three-dimensional space.

7. The linear rigid body motion tracking method based on super-resolution according to claim 6, characterized in that: The step (3) specifically comprises: (31) According to the rotation fulcrum of the rigid body and the detection result of the previous frame, the possible appearance area of ​​the rigid body in the current frame is determined as the detection area of ​​interest, and the plane angle calculation result α of the previous two frames is taken -2 and α -1 , determine the deflection angle of the rigid body between the two frames, which is the motion angular velocity vector V containing the motion direction information, as follows: V＝α -1 -α -2 Get and refine the specific angle range of the region of interest ROI. For the direction of motion of the rigid body, start from the rotation angle of the rigid body in the previous frame and detect the angle of the motion direction. The angle range of the detection depends on the angular velocity V of the rigid body in the previous frame. Perform a small range check in the opposite direction to prevent the rigid body from changing its motion direction suddenly. If the current rigid body is stationary, perform the same range of detection for both clockwise and counterclockwise directions: Among them, k,θ are hyperparameters used to cope with the change of rigid body angular velocity, ∈ is the minimum unit of angle change during line detection, which is used to cope with the situation where the rigid body's motion direction changes suddenly; (32) According to the rotation fulcrum of the rigid body and the region of interest, the rigid body is detected from the region of interest by straight line detection with a known endpoint. At the same time, the motion information of the rigid body is combined to eliminate background interference and the straight line detection result L is calculated. i The frame difference result D of two adjacent frames i Take the intersection and use the motion information of the rigid body to obtain a more robust detection result, as follows: L i =L i ∩D i Through the above straight line detection process, the angle between the corresponding straight line of the rigid body and the horizontal plane is the plane rotation angle α of the rigid body, and the length of the straight line is the length S of the rigid body. k ; (33) Select the stationary rigid body as the reference frame and use the inertial sensor to obtain the angle β with the camera plane ′ , use the magnetometer to select an initial vector v0 in the three-dimensional space, and then obtain the rotation matrix A1 of v0 to the same direction as the rigid body in the reference frame and the rotation matrix A2 to the same direction as the camera, and use the rotation matrix to obtain the three-dimensional vectors v1 and v2 representing the rigid body direction and the camera direction in the three-dimensional space, as follows: v i =A i v0,i∈[1,2] According to the three-dimensional vectors of the rigid body and the camera, the angle β′ between the rigid body and the camera plane is obtained as follows: According to the rigid body length S in the reference frame k Calculate the reference length S′ i ,as follows: (34) According to the length S of rigid body i in the current frame i and its reference length S′ i The vertical angle β is calculated as follows: Thus, the three-dimensional spatial motion tracking of the rigid body is completed, and the plane rotation angle α and the vertical rotation angle β of the rigid body are obtained.

Images