Method and apparatus for calibrating three-dimensional coordinates of output space using six-axis IMU output data

Abstract

This invention obtains the sub-pixel coordinates of the geometric center of the marker point in a two-dimensional image from the video stream data acquired by the camera. After image preprocessing and connected component extraction, the sub-pixel coordinates of the marker point's geometric center are calculated at the PL end and stereo matching of the sub-pixel coordinates of the marker point is achieved at the PS end. Therefore, it can determine whether there is vibration in the optical tracking system and provide a warning. It can also calculate the rotation and translation matrix of the system relative to the original pose after the pose is changed due to vibration based on the six-axis IMU data, and then automatically calibrate and output the correct three-dimensional spatial coordinates.

Description

Technical Field

[0001] This invention relates to the field of medical image processing technology, and more particularly to a method for calibrating the three-dimensional coordinates of the output space of a six-axis IMU, and a device for calibrating the three-dimensional coordinates of the output space of a six-axis IMU, which is mainly applicable to the field of surgical navigation. Background Technology

[0002] The positioning device is the most crucial component of a surgical navigation system. Its real-time performance and accuracy directly impact the safety and precision of the surgical navigation system, thus affecting the final surgical success rate. The positioning technologies used in surgical navigation devices mainly include four types: mechanical positioning systems, ultrasonic positioning systems, electromagnetic positioning systems, and optical positioning systems.

[0003] While optical positioning systems offer high positioning accuracy, their measurements of three-dimensional coordinates are most accurate when the system remains stationary. However, optical positioning systems often experience minor vibrations during measurement, and existing systems cannot provide timely vibration alerts. This prevents testers from making timely adjustments, further contributing to inaccurate measurement results. Summary of the Invention

[0004] To overcome the shortcomings of the prior art, the technical problem to be solved by the present invention is to provide a method for calibrating the output three-dimensional coordinates of a six-axis IMU output data. This method can determine whether there is vibration in the optical tracking system and provide a reminder. It can also calculate the rotation and translation matrix of the system relative to the original pose after the pose is changed due to vibration based on the six-axis IMU data, and then automatically calibrate and output the correct three-dimensional coordinates.

[0005] The technical solution of this invention is: a method for calibrating the three-dimensional coordinates of the output space of a six-axis IMU, which includes the following steps:

[0006] (1) The video stream data acquired from the camera is processed by image preprocessing and connected component extraction to obtain the sub-pixel coordinates of the geometric center of the marker point in the two-dimensional image. The coordinate values ​​are stored in Block RAM. The PS end reads the coordinate values ​​through AXI Block RAMController. The video stream data is converted into AXI4-Stream IP format data stream by the Video in to AXI4-Stream IP core. The VDMA IP core stores the data in DDR3 memory through the HP interface of the AXI Smartconnect bus. The AXI Interconnect bus realizes the interconnection between the GP interface and the peripheral configuration interface, thereby realizing the PS end to control the peripherals of the PL end.

[0007] (2) The sub-pixel coordinates of the geometric center of the marker point are calculated on the PL end. Assuming that there are n marker points installed on the surgical instrument, there are corresponding n light spot regions in the images captured by the left and right cameras. Suppose that the gray values ​​of m pixels in the j-th (j=1,2…n) light spot region are greater than a specified threshold t, where the coordinates of the i-th (i=1,2…m) pixel are represented as q. i =[q ix ,q iy ] T And the grayscale value of this pixel is represented as v i Then the sub-pixel coordinates q of the light spot i From the following formula, we get

[0008]

[0009] (3) The stereo matching of sub-pixel coordinates of marker points is implemented on the PS end. After stereo matching is completed based on the principle of epipolar geometric constraints, the three-dimensional coordinates of the marker points in the actual space are obtained through the three-dimensional reconstruction of spatial points.

[0010] This invention obtains the sub-pixel coordinates of the geometric center of the marker point in a two-dimensional image from the video stream data acquired by the camera. After image preprocessing and connected component extraction, the sub-pixel coordinates of the marker point's geometric center are calculated at the PL end and stereo matching of the sub-pixel coordinates of the marker point is achieved at the PS end. Therefore, it can determine whether there is vibration in the optical tracking system and provide a warning. It can also calculate the rotation and translation matrix of the system relative to the original pose after the pose is changed due to vibration based on the six-axis IMU data, and then automatically calibrate and output the correct three-dimensional spatial coordinates.

[0011] It also provides a device for calibrating the three-dimensional coordinates of the output space of a six-axis IMU, which includes:

[0012] The acquisition and preprocessing module is configured to process video stream data acquired from the camera, perform image preprocessing and connected component extraction to obtain the sub-pixel coordinates of the geometric center of the marker points in the two-dimensional image, and store the coordinate values ​​in Block RAM. The PS end reads the coordinate values ​​through the AXI Block RAM Controller. The video stream data is converted into AXI4-Stream IP format data stream by the Video in to AXI4-Stream IP core, and stored in DDR3 memory by the VDMA IP core through the HP interface of the AXI Smartconnect bus. The AXI Interconnect bus realizes the interconnection between the GP interface and the peripheral configuration interface, thereby enabling the PS end to control the peripherals of the PL end.

[0013] The calculation module is configured to calculate the sub-pixel coordinates of the geometric center of the marker points on the PL end. Assuming there are n marker points installed on the surgical instrument, there are corresponding n light spot regions in the images captured by the left and right cameras. Let m pixels in the j-th (j = 1, 2…n) light spot region have gray values ​​greater than a specified threshold t, where the coordinates of the i-th (i = 1, 2…m) pixel are represented by q. i =[q ix ,q iy ] T And the grayscale value of this pixel is represented as v i Then the sub-pixel coordinates q of the light spot i From the following formula, we get

[0014]

[0015] The matching module is configured to perform stereo matching of sub-pixel coordinates of marker points on the PS side. After completing stereo matching based on the principle of epipolar geometric constraints, the three-dimensional coordinates of the marker points in the actual space are obtained through the three-dimensional reconstruction of spatial points. Attached Figure Description

[0016] Figure 1 This is a flowchart of a method for calibrating the three-dimensional coordinates of the output space using six-axis IMU output data according to the present invention.

[0017] Figure 2 This is a flowchart of the calculation of sub-pixel coordinates of the geometric center of the marker point according to the present invention.

[0018] Figure 3 This is a flowchart of the stereo matching process for the coordinates of two-dimensional image marker points according to the present invention.

[0019] Figure 4 This is a schematic diagram of the position of the optical tracking system according to the present invention after rotating by an angle θ. Detailed Implementation

[0020] like Figure 1 As shown, this method for calibrating the three-dimensional coordinates of the output space using six-axis IMU output data includes the following steps:

[0021] (1) The video stream data acquired from the camera is processed by image preprocessing and connected component extraction to obtain the sub-pixel coordinates of the geometric center of the marker point in the two-dimensional image. The coordinate values ​​are stored in Block RAM. The PS end reads the coordinate values ​​through AXI Block RAMController. The video stream data is converted into AXI4-Stream IP format data stream by the Video in to AXI4-Stream IP core. The VDMA IP core stores the data in DDR3 memory through the HP interface of the AXI Smartconnect bus. The AXI Interconnect bus realizes the interconnection between the GP interface and the peripheral configuration interface, thereby realizing the PS end to control the peripherals of the PL end.

[0022] (2) The sub-pixel coordinates of the geometric center of the marker point are calculated on the PL end. Assuming that there are n marker points installed on the surgical instrument, there are corresponding n light spot regions in the images captured by the left and right cameras. Suppose that the gray values ​​of m pixels in the j-th (j=1,2…n) light spot region are greater than a specified threshold t, where the coordinates of the i-th (i=1,2…m) pixel are represented as q. i =[q ix ,q iy ] T And the grayscale value of this pixel is represented as v i Then the sub-pixel coordinates q of the light spot i From the following formula, we get

[0023]

[0024] (3) The stereo matching of sub-pixel coordinates of marker points is implemented on the PS end. After stereo matching is completed based on the principle of epipolar geometric constraints, the three-dimensional coordinates of the marker points in the actual space are obtained through the three-dimensional reconstruction of spatial points.

[0025] This invention obtains the sub-pixel coordinates of the geometric center of the marker point in a two-dimensional image from the video stream data acquired by the camera. After image preprocessing and connected component extraction, the sub-pixel coordinates of the marker point's geometric center are calculated at the PL end and stereo matching of the sub-pixel coordinates of the marker point is achieved at the PS end. Therefore, it can determine whether there is vibration in the optical tracking system and provide a warning. It can also calculate the rotation and translation matrix of the system relative to the original pose after the pose is changed due to vibration based on the six-axis IMU data, and then automatically calibrate and output the correct three-dimensional spatial coordinates.

[0026] Preferably, in step (1), the frame buffer space size, read / write channels, and received coordinate calculation completion signal of the VDMA IP core are configured. The value of the register in the IP core of the PL end is modified on the PS end through the AXI Interconnect interface, thereby controlling the exposure mode, exposure time, lighting time of the near-infrared LED, and adjusting the threshold of connected component extraction of the image sensor.

[0027] Preferably, in step (2), multiple marker points are installed on a surgical instrument, and each pixel point greater than the threshold is divided into the corresponding light spot area using the gray threshold t, the distance threshold l, and the adjacent interval m. If a point that meets the conditions is found, its neighborhood m refers to the rectangular area with the upper left vertex of the point and a side length of 1×m.

[0028] Preferably, in step (3), p1 and p2 are image points of arbitrary spatial point P on the left and right cameras, respectively, and their pixel coordinates have been obtained; assuming the three-dimensional spatial coordinates of P are (X... W ,Y W Z W The pixel coordinates of p1 and p2 are (u1, v1) and (u2, v2) respectively, and the projection matrices of the two cameras are as follows:

[0029]

[0030]

[0031] Calculate the three-dimensional spatial coordinates of P as (X W ,Y W Z W The four linear equations are:

[0032]

[0033]

[0034] Preferably, in step (3), formulas (4) and (5) represent straight lines passing through O1p1 and O2p2 respectively, and point P is the intersection of the two. Therefore, both formulas are satisfied simultaneously, and the coordinates of point P in the world coordinate system can be solved by simultaneously solving the two formulas. Because the above two sets of equations contain a total of four linear equations and three unknowns, the three equations can be solved to obtain (X... W ,Y W Z W The value of ); In reality, the presence of noise affects the data obtained by PS processing, so the least squares method is used to solve for the three-dimensional coordinates of spatial points.

[0035] Preferably, in step (1), the six-axis IMU uses an MPU-6050 inertial measurement unit to output three-axis accelerometer data and three-axis gyroscope data. When the optical tracking system vibrates and changes its posture, the PS end compares the acceleration data in the three directions of X, Y, and Z output by the MPU-6050 inertial measurement unit with the data of gravitational acceleration to calculate its posture. The comparison of the posture before and after the movement yields the rotation and translation matrix of the system before and after the movement.

[0036] Preferably, in step (3), the optical tracking system rotates by an angle θ relative to its original position. At this time, the data output reflected on the MPU-6050 inertial measurement unit is g. x g y g z , where g y =0, g z =g cos(θ), g x = g sin(θ), according to the above formula, θ is calculated. Due to structural limitations, the distance between the optical tracking system and the rotation axis is constant. Based on the rotation angle θ and the distance between the optical tracking system and the rotation axis, the rotation and translation matrix of the system relative to its original position is obtained, thereby transforming the collected three-dimensional coordinate data of the marker points to the same coordinate system.

[0037] Those skilled in the art will understand that all or part of the steps in the methods of the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium. When executed, the program includes the steps of the methods of the above embodiments. The storage medium can be ROM / RAM, magnetic disk, optical disk, memory card, etc. Therefore, corresponding to the method of the present invention, the present invention also includes a device for calibrating the three-dimensional coordinates of the output space of a six-axis IMU. This device is typically represented in the form of functional modules corresponding to the steps of the method. The device includes:

[0038] The acquisition and preprocessing module is configured to process video stream data acquired from the camera, perform image preprocessing and connected component extraction to obtain the sub-pixel coordinates of the geometric center of the marker points in the two-dimensional image, and store the coordinate values ​​in Block RAM. The PS end reads the coordinate values ​​through the AXI Block RAM Controller. The video stream data is converted into AXI4-Stream IP format data stream by the Video in to AXI4-Stream IP core, and stored in DDR3 memory by the VDMA IP core through the HP interface of the AXI Smartconnect bus. The AXI Interconnect bus realizes the interconnection between the GP interface and the peripheral configuration interface, thereby enabling the PS end to control the peripherals of the PL end.

[0039] The calculation module is configured to calculate the sub-pixel coordinates of the geometric center of the marker points on the PL end. Assuming there are n marker points installed on the surgical instrument, there are corresponding n light spot regions in the images captured by the left and right cameras. Let m pixels in the j-th (j = 1, 2…n) light spot region have gray values ​​greater than a specified threshold t, where the coordinates of the i-th (i = 1, 2…m) pixel are represented by q. i =[q ix ,q iy ] T And the grayscale value of this pixel is represented as v i Then the sub-pixel coordinates q of the light spot i From the following formula, we get

[0040]

[0041] The matching module is configured to perform stereo matching of sub-pixel coordinates of marker points on the PS side. After completing stereo matching based on the principle of epipolar geometric constraints, the three-dimensional coordinates of the marker points in the actual space are obtained through the three-dimensional reconstruction of spatial points.

[0042] Preferably, in the acquisition and preprocessing module, the six-axis IMU is an MPU-6050 inertial measurement unit, which outputs three-axis accelerometer data and three-axis gyroscope data. When the optical tracking system vibrates and changes its posture, the PS terminal compares the acceleration data in the X, Y, and Z directions output by the MPU-6050 inertial measurement unit with the gravitational acceleration data to calculate its posture. The comparison of the posture before and after the movement yields the rotation and translation matrix of the system before and after the movement.

[0043] Preferably, the device further includes a fault alarm and indicator light circuit, using three indicator lights to represent the system's working status: a green light for power, a yellow light for working status, and a red light for fault alarm.

[0044] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Any simple modifications, equivalent changes, and alterations made to the above embodiments based on the technical essence of the present invention shall still fall within the protection scope of the present invention.

Claims

1. A method for calibrating output data of a six-axis IMU to output three-dimensional coordinates in a space, characterized by comprising the following steps: (1) obtaining the geometric center sub-pixel coordinates of the marker points in a two-dimensional image from the video stream data collected by a camera through image preprocessing and connected domain extraction, storing the coordinate values in a Block RAM, reading the coordinate values by a PS end through an AXI Block RAM Controller, converting the video stream data into AXI4-Stream IP format data stream by a Video in to AXI4-Stream IP core, storing the data stream in a DDR3 memory through the HP interface of an AXI Smartconnect bus by a VDMA IP core, and realizing the interconnection of the GP interface and the peripheral configuration interface through an AXI Interconnect bus, so as to realize the control of the peripherals of the PL end by the PS end; (3) realizing the stereo matching of the sub-pixel coordinates of the marker points in the PS end, and obtaining the three-dimensional coordinates of the marker points in the actual space through the three-dimensional reconstruction of the space points after completing the stereo matching based on the principle of geometric constraint of epipolar line. (2) The calculation of the geometric center sub-pixel coordinates of the marker points is implemented at the PL end. Assuming that n marker points are installed on the surgical instrument, there are corresponding n light point regions in the images captured by the left and right cameras. Assuming that the gray scale values of m pixel points in the jth (j = 1, 2…n) light point region are greater than a specified threshold t, the coordinates of the ith (i = 1, 2…m) pixel point are represented as q i = [q ix x , q iy y ] T , and the gray scale value of the pixel point is represented as v i , then the sub-pixel coordinates q i of the light point are obtained from the following formula In the step (1), the frame buffer space size, read-write channel and signal of receiving coordinate calculation of the VDMA IP core are configured, the values of the registers in the IP core of the PL end are modified in the PS end through the AXI Interconnect interface, so as to control the exposure mode, exposure time, lighting time of the near-infrared LED lamp of the image sensor, and adjust the threshold value of the connected domain extraction.

2. The method of calibrating the output space three-dimensional coordinates of the six-axis IMU output data according to claim 1, characterized in that: In the step (2), a plurality of marker points are installed on a surgical instrument, and each pixel point greater than the threshold value is divided into a corresponding light point region by using the gray threshold value t, the distance threshold value l and the adjacent interval m, wherein the adjacent interval m of a point meeting the condition is a rectangular region with the top left vertex of the point and the side length of 1×m.

3. The method of calibrating the output space three-dimensional coordinates of the six-axis IMU output data according to claim 2, characterized in that: In the step (1), the six-axis IMU selects an MPU-6050 inertial measurement unit, outputs three-axis accelerometer data and three-axis gyroscope data, when the optical tracking system is vibrated to change the pose, the PS end calculates the attitude according to the acceleration data of X, Y and Z directions output by the MPU-6050 inertial measurement unit and the data of gravitational acceleration, and obtains the rotation and translation matrix of the system before and after the motion through the comparison of the attitudes before and after the motion.

4. The method of calibrating the output space three-dimensional coordinates of the six-axis IMU output data according to claim 3, characterized in that: In the step (3), let p1 and p2 be the image points of an arbitrary spatial point P on the left and right cameras respectively, and the pixel coordinates of both have been obtained; suppose the spatial three-dimensional coordinates of P are (X W ,Y W ,Z W ), the pixel coordinates of p1 and p2 are (u1, v1) and (u2, v2) respectively, and the projection matrices of the two cameras are: The four linear equations for calculating the spatial three-dimensional coordinates of P (X W ,Y W ,Z W ) are:

5. The method of calibrating the output space three-dimensional coordinates of the six-axis IMU output data according to claim 4, characterized in that: In the step (3), the formulas (4) and (5) respectively represent straight lines passing through O1p1 and O2p2, and point P is the intersection of the two, so the two formulas are satisfied at the same time, and the coordinates of point P in the world coordinate system are solved by combining the two formulas; because the above two equations contain four linear equations in total and have three unknowns, the values of (X W ,Y W ,Z W ) are solved by three equations; in actual situations, the presence of noise affects the data obtained by the PS end processing, and the least square method is used to solve the three-dimensional coordinates of the space point.

6. The method of calibrating the output space three-dimensional coordinates of six-axis IMU output data according to claim 1, characterized in that: It comprises:

7. The method of calibrating the output space three-dimensional coordinates of the six-axis IMU output data according to claim 6, characterized in that: In the step (3), the optical tracking system rotates by an angle of θ relative to the original position, at this time, the data output reflected on the MPU-6050 inertial measurement unit is g x , g y , g z , wherein g y =0, g z =gcos(θ), g x =gsin(θ), according to the above formula, θ is calculated, due to the limitation of the structure, the distance between the optical tracking system and the rotation shaft is constant, according to the rotation angle θ and the distance between the optical tracking system and the rotation shaft, the rotation translation matrix of the system relative to the original position is obtained, so as to convert the collected marker point three-dimensional coordinate data to the same coordinate system.

8. Apparatus for calibrating six-axis IMU output data to output spatial three-dimensional coordinates, characterized by: ​ The acquisition preprocessing module is configured to obtain the geometric center sub-pixel coordinates of the mark points of the two-dimensional image through image preprocessing and connected domain extraction from the video stream data obtained from the camera, and store the coordinate values in the Block RAM. The PS end reads the coordinate values through the AXI Block RAM Controller. The video stream data is converted into AXI4-Stream IP format data stream through the Video in to AXI4-Stream IP core, and is stored in the DDR3 memory through the HP interface of the AXI Smartconnect bus by the VDMA IP core. The AXI Interconnect bus realizes the interconnection of the GP interface and the peripheral configuration interface, so as to realize the control of the PS end on the peripherals of the PL end. The computing module is configured to mark the point geometric center sub-pixel coordinate calculation is realized at the PL end. Assuming that n marking points are installed on the surgical instrument, there are corresponding n light point areas in the images captured by the left and right cameras. It is assumed that there are m pixel points in the jth (j=1, 2…n) light point area whose gray values are greater than a specified threshold t, wherein the coordinates of the ith (i=1, 2…m) pixel point are represented as q i =[q ix , q iy ] T , and the gray value of the pixel point is represented as v i . The sub-pixel coordinates q i of the light point are obtained from the following formula The matching module is configured to realize the stereo matching of the sub-pixel coordinates of the mark points in the PS end. After the stereo matching is completed based on the principle of the geometric constraint of the epipolar line, the three-dimensional coordinates of the mark points in the actual space are obtained through the three-dimensional reconstruction of the space points.

9. The apparatus for calibrating six-axis IMU output data to output spatial three- dimensional coordinates of claim 8, wherein: In the acquisition preprocessing module, the six-axis IMU selects the MPU-6050 inertial measurement unit, and outputs three-axis accelerometer data and three-axis gyroscope data. When the optical tracking system changes the pose due to vibration, the PS end compares the acceleration data of X, Y and Z directions output by the MPU-6050 inertial measurement unit with the data of the gravitational acceleration to calculate the attitude, and compares the attitudes before and after the movement to obtain the rotation and translation matrix of the system before and after the movement.

10. The apparatus for calibrating six-axis IMU output data to output spatial three- dimensional coordinates of claim 9, wherein: The device further comprises a fault alarm and indicator light circuit, which represents the working state of the system through three indicator lights. The green light is the power indicator light, the yellow light is the working state indicator light, and the red light is the fault alarm light.

Images