A method for spatio-temporal co-calibration of a magnetic sensor and an endoscope

Abstract

The application provides a space-time joint calibration method for a magnetic sensor and an endoscope, and relates to medical robot technology, and steps include: collecting endoscope images and magnetic positioning information (time stamp position is not synchronized); acquiring internal and external parameters and distortion parameters of the endoscope images based on a camera calibration method of feature points; interpolating the magnetic positioning information with an image timestamp as a target timestamp; outputting a preliminary estimated external transfer matrix between sensors and a scale expansion coefficient between trajectories through a hand-eye calibration method with uncertain translation scale; and obtaining an optimal solution of a systematic time offset between the endoscope and the sensor and an external transfer matrix (i.e. a relative pose relationship between the endoscope lens and the magnetic sensor) by using a proposed space-time calibration parameter optimization algorithm based on a sliding window. The application can realize space-time joint calibration of the endoscope and the magnetic sensor without the need of hardware synchronization, and is convenient for users to flexibly assemble the endoscope and the magnetic sensor.

Description

TECHNICAL FIELD

[0001] The present application relates to medical robot technology, in particular to a magnetic sensor and endoscope spatio-temporal joint calibration method. BACKGROUND

[0002] Multi-sensor fusion is very common in medical robot application scenarios. In tasks involving visual perception, endoscopes can obtain observation pictures, and magnetic positioning sensors can continuously locate and track in real time. In order to obtain a consistent description of the observation environment, fusion perception is needed. The premise of the effectiveness of such multi-source heterogeneous sensor fusion algorithm is that the precise spatio-temporal calibration between sensors is completed. To meet this requirement, the traditional sensor synchronization scheme requires that the endoscope and the magnetic positioning sensor be hardware synchronized in terms of time, and that the relative pose relationship between the sensors be assembled and debugged by professional technicians before leaving the factory in terms of space.

[0003] Hardware synchronization requires the use of a hardware trigger to directly trigger the endoscope and the magnetic positioning sensor through a physical signal, so that they can sample at the same time. However, in actual application scenarios, this may be affected by the triggering mechanism, data delay, etc. The strict spatial pose requirement also makes the multiple sensors be firmly bound together, and multiple similar devices cannot be freely assembled with sensors, which has poor scalability and flexibility, causing great difficulty in cleaning, disinfecting, and replacing the endoscope lens. Considering the influence of long-term operation and temperature and other factors, after the related parameters of the sensor change, it is difficult for the operator to calibrate, and often needs to be returned to the factory for repair, which has low stability and high cost. SUMMARY

[0004] The present application proposes a magnetic sensor and endoscope spatio-temporal joint calibration method, which can easily complete the calibration work by operators without professional technical background, so that the endoscope and the magnetic sensor do not need hardware synchronization, and realize plug and play and flexible assembly. The present spatio-temporal joint calibration method can calibrate the following at one time: the intrinsic parameters and distortion coefficients of the endoscope lens; the systematic time offset relationship between the endoscope image acquisition and the magnetic sensor positioning information; and the relative pose relationship between the endoscope lens and the magnetic sensor.

[0005] The technical scheme adopted by the present application is as follows:

[0006] A magnetic sensor and endoscope spatio-temporal joint calibration method, comprising the following steps:

[0007] Step 1: The operator manipulates the endoscope equipped with a magnetic sensor to observe a calibration board or other objects with known specific dimensions, collects endoscope images and magnetic positioning information, and records the time stamp t e-ts of the electromagnetic sensor and the time stamp t c-ts(both time is not synchronized, and the timestamp and the real time has time offset).

[0008] Step 2: based on the feature point camera calibration method, obtain the internal and external parameters and distortion parameters of the endoscope image.

[0009] Further, the specific steps of the camera calibration method based on the feature points in step 2 to obtain the internal and external parameters and distortion parameters of the endoscope image are:

[0010] Step 2.1: input the endoscope image, obtain the feature point matching pair through the feature extraction and matching algorithm, and record the pixel coordinates of the feature points in the matching pair;

[0011] Step 2.2: calculate the physical coordinates of each feature point in the world coordinate system through the size information of the known size calibration board or other known objects;

[0012] Step 2.3: input the pixel coordinates and physical coordinates of the feature points into the camera imaging model, solve the internal and external parameters of the camera, and estimate the distortion parameters [k1, k2, k3, p1, p2] through the least square method.

[0013] Step 3: take the image timestamp t c-ts as the target timestamp, interpolate the magnetic positioning information, so that the timestamps of the image and the positioning information become synchronized (at this time, the image and the pose information are not really aligned in time).

[0014] Further, the specific steps of step 3 of taking the image timestamp as the target timestamp and interpolating the magnetic positioning information are:

[0015] Step 3.1: interpolate the position information of the magnetic positioning through the position interpolation extrapolation method, so that the timestamps of the image and the position information become synchronized;

[0016] Step 3.2: interpolate the attitude information of the magnetic positioning through the attitude spherical linear interpolation method, so that the timestamps of the image and the magnetic sensor attitude information become synchronized.

[0017] Step 4: input the endoscope lens external parameter trajectory obtained in step 2 and the magnetic positioning trajectory obtained in step 3, and output the preliminary estimated external transfer matrix T c→e between the endoscope and the magnetic sensor and the scale expansion coefficient s between the trajectories through the hand-eye calibration method with uncertain translation scale.

[0018] Further, the specific steps of step 4 of outputting the preliminary estimated external transfer matrix T c→e between the endoscope and the magnetic sensor and the scale expansion coefficient s between the trajectories through the hand-eye calibration method with uncertain translation scale are:

[0019] Step 4.1: Represent the translation in the extrinsic parameters of the endoscope as a product of a scale factor and a translation matrix:

[0020]

[0021] where s represents the scale factor.

[0022] Step 4.2: Convert the hand-eye calibration equation with scale factor into a quadratic constraint quadratic programming form:

[0023]

[0024] where R c→e and t c→e represent the rotation matrix and translation matrix between the endoscope and the sensor, R c and t c represent the rotation matrix and translation matrix of the endoscope, R e and t e represent the rotation matrix and translation matrix of the electromagnetic sensor.

[0025] Step 4.3: Unfold the rotation matrix R c→e between the endoscope and the sensor into a one-dimensional vector V c→e in row order, let x T = [t c→e T s V c→e ], convert the equation of step 4.3 into the form of x T Qx, where Q represents:

[0026]

[0027] where Q s is a matrix related to the scale factor s, Q V is a matrix related to the one-dimensional vector V c→e , and Q s,V is a matrix related to both the scale factor s and the one-dimensional vector V c→e .

[0028] Step 4.4: Calculate the translation matrix t c→e between the endoscope and the sensor and the scaling factor s between the trajectory based on step 4.3:

[0029]

[0030] Step 4.5: Bring the result of step 4.4 into the formula of step 4.3, convert it into a form including only rotation variables, and optimize it through a set of data equations to obtain a preliminary estimate of the external transformation matrix T between the endoscope and the magnetic sensor c→e and the scale factor s between the trajectories.

[0031] Step 5: Finally, use the proposed sliding window-based spatiotemporal calibration parameter optimization algorithm to obtain the optimal solution of the systematic time offset between the endoscope and the sensor and the external transformation matrix (i.e., the relative pose relationship between the endoscope lens and the magnetic sensor).

[0032] Further, the specific steps of the proposed sliding window-based spatiotemporal calibration parameter optimization algorithm in step 5 are as follows:

[0033] Step 5.1: Based on the preliminary estimate of the external transformation matrix between the sensors and the scale factor s between the trajectories output in step 4, set the time offset interval [t1 t n ] and the offset step t step to traverse different time offset values, compensate the time stamps corresponding to the magnetic positioning information obtained in step 1, repeat steps 3 and 4, and record the newly obtained external transformation matrix and the scale factor s between the trajectories;

[0034] Step 5.2: Statistically analyze the scale factors s i obtained in step 5.1 when the magnetic positioning information time stamps are compensated for different time offsets t i , obtain the maximum value s max of the scale factors in the interval, and set s max and its corresponding external transformation matrix as the currently estimated best scale factor and external transformation matrix.

[0035] Step 5.3: Set a given parameter precision threshold, repeat steps 5.1 and 5.2 until the changes of each parameter in the external matrix and the scale factor are less than the set threshold, to obtain the systematic time offset between the endoscope and the sensor and the final external transformation matrix (i.e., the relative pose relationship between the endoscope lens and the magnetic sensor).

[0036] Advantages and benefits of the present application:

[0037] The present application performs spatiotemporal calibration between sensors based on software synchronization, considers the time offset of endoscope and magnetic sensor data frames, and allows non-technical personnel to easily complete the calibration work, which is convenient to operate, low in cost, and can realize plug-and-play and flexible assembly. BRIEF DESCRIPTION OF DRAWINGS

[0038] Figure 1 is the overall flowchart of the spatiotemporal calibration method of the present application;

[0039] Figure 2 is a flow chart of the method for obtaining the endoscope image's intrinsic and extrinsic parameters and distortion parameters in step 2;

[0040] Figure 3 is a schematic diagram of synchronizing the timestamps of the image and the positioning information in step 3;

[0041] Figure 4 is a schematic diagram of the spherical linear interpolation method for the pose in step 3

[0042] Figure 5 is a schematic diagram of the spatial relationship between the magnetic sensor and the endoscope;

[0043] Figure 6 is a flow chart of the spatiotemporal calibration parameter optimization algorithm of the sliding window in step 5;

[0044] Figure 7 is a schematic diagram of observing the checkerboard calibration board and recording the timestamps;

[0045] Figure 8 is a schematic diagram of the checkerboard feature extraction;

[0046] Figure 9 is the scale scaling coefficient obtained by compensating for the different time offsets of the timestamps of the magnetic positioning information. DETAILED DESCRIPTION

[0047] The specific embodiments of the present application will be further described in detail below in conjunction with the accompanying drawings and specific embodiments. The following examples are used to illustrate the present application, but are not used to limit the scope of the present application.

[0048] Example 1:

[0049] The present application discloses a spatiotemporal joint calibration method for a magnetic sensor and an endoscope, and the method flow is as shown in Figure 1 The following system can be used:

[0050] The system mainly consists of an endoscope, a magnetic sensor, and a computer. The endoscope is operated by a professional, responsible for collecting image information in the field of view and transmitting it to the computer. The magnetic sensor is responsible for collecting pose information and transmitting it to the computer. The computer calibrates the intrinsic parameters, distortion coefficients of the endoscope lens, the systematic time offset relationship between the endoscope image acquisition and the magnetic sensor positioning information, and the relative pose relationship between the endoscope lens and the magnetic sensor by the method disclosed in the present application.

[0051] As shown in Figure 1 The spatiotemporal joint calibration method for a magnetic sensor and an endoscope disclosed in the present application includes the following steps:

[0052] Step 1: The operator manipulates the endoscope equipped with magnetic sensors to observe the calibration board or other known specific size objects, collects endoscopic images and magnetic positioning information, and records the timestamps t of the electromagnetic sensors in real time e-ts and the timestamps t of the images c-ts (where both time is not synchronized, and the timestamps have a time offset from the real time). This embodiment takes a 25*25 chessboard calibration board as an example, and the size of the squares on the calibration board is 2mm*2mm, as shown in Figure 7 Figure 7 The three pictures have different shooting times, and their names reflect the time sequence. Eighteen endoscopic images and magnetic positioning information in the same period are collected, and the timestamps of the two are recorded.

[0053] Step 2: Based on the chessboard calibration method, the intrinsic and extrinsic parameters and distortion parameters of the endoscopic image are obtained. Specifically, as shown in Figure 2 the steps of obtaining the intrinsic and extrinsic parameters and distortion parameters of the endoscopic image based on the camera calibration method of feature points are as follows:

[0054] Step 2.1: Input the endoscopic image, obtain the feature point matching pairs through the SURF feature extraction and matching algorithm, record the pixel coordinates of the feature points in the matching pairs, and extract the features as shown in Figure 8

[0055] Step 2.2: Calculate the physical coordinates of each feature point in the world coordinate system through the size information of the known calibration board;

[0056] Step 2.3: Input the pixel coordinates and physical coordinates of the feature points into the pinhole camera imaging model to solve the intrinsic matrix of the endoscope:

[0057]

[0058] At the same time, record the translation matrix and rotation matrix of the 18 images, and estimate the distortion parameters through the least squares method:

[0059] [k1, k2, k3, p1, p2]

[0060] Where:

[0061] k1 = -0.08259243160139441

[0062] k2 = -0.2721349814084781

[0063] k3 = 0.0006032726042589804

[0064] p1 = -0.0006098617993084006

[0065] ​​p2 = 0.2182103661437886

[0066] Step 3: Interpolate the magnetic localization information with the image timestamp t c-ts as the target timestamp, so that the timestamps of the image and the localization information become synchronized (at this time, the image and the pose information are not really aligned in time), as shown in Figure 3 . Specifically, the specific steps of interpolating the magnetic localization information with the image timestamp as the target timestamp are as follows:

[0067] Step 3.1: Interpolate the position information of the magnetic localization by the position interpolation extrapolation method, i.e., find the corresponding position information of the magnetic sensor time axis t e-true in the image time axis t c-true , so that the timestamps of the image and the position information become synchronized;

[0068] Step 3.2: Interpolate the pose information of the magnetic localization by the pose spherical linear interpolation method, i.e., find the corresponding pose information q(t) of the magnetic sensor time axis, and find q(t) in the image time axis using the pose information q0, q1 adjacent to it in the image time axis, so that the timestamps of the image and the pose information become synchronized, as shown in Figure 4 .

[0069] Step 4: Input the endoscope lens extrinsic parameter trajectory obtained in step 2.3 and the magnetic localization trajectory obtained in step 3, as shown in Figure 5 , and output the preliminary estimated external transfer matrix T c→e between the endoscope and the magnetic sensor and the scale scaling factor s between the trajectories by the monocular hand-eye calibration method. Specifically, the specific steps of outputting the preliminary estimated external transfer matrix T c→e between the endoscope and the magnetic sensor and the scale scaling factor s between the trajectories by the translation scale uncertain hand-eye calibration method are as follows:

[0070] Step 4.1: Express the translation amount in the endoscope extrinsic parameter as the product of the scale scaling factor and the translation matrix:

[0071]

[0072] where s represents the scale scaling factor.

[0073] Step 4.2: Convert the hand-eye calibration formula containing the scale scaling factor into a quadratic constraint quadratic programming form:

[0074]

[0075] where R c→e and t c→eRend and tden, respectively, denote the rotation matrix and translation matrix between the endoscope and the sensor, R c and t c , respectively, denote the rotation matrix and translation matrix of the endoscope, R e and t e , respectively, denote the rotation matrix and translation matrix of the electromagnetic sensor.

[0076] Step 4.3: Unfold the rotation matrix R c→e between the endoscope and the sensor into a one-dimensional vector V c→e in row order, let x T = [t c→e T s V c→e ], and convert the equation of step 4.3 into the form of x T Qx, let Q denote in it as:

[0077]

[0078] where Q s is a matrix related to the scale factor s, Q V is a matrix related to the one-dimensional vector V c→e , and Q s,V is a matrix related to both the scale factor s and the one-dimensional vector V c→e .

[0079] Step 4.4: Based on step 4.3, calculate the translation matrix t c→e between the endoscope and the sensor and the scale factor s between the trajectory:

[0080]

[0081] Step 4.5: Bring the results of step 4.4 into the step 4.3 formula, convert it into a form that only includes rotation variables, and optimize it through a set of data equations to obtain the preliminary estimated external transfer matrix T c→e between the endoscope and the magnetic sensor and the scale factor s between the trajectory.

[0082] Output the preliminary estimated external rotation matrix between the sensors as:

[0083]

[0084] The external translation matrix is:

[0085] [-0.012263376 0.020017566 -0.0513048]

[0086] The scale factor between the trajectories is 0.966.

[0087] Step 5: As Figure 6 As shown, the proposed sliding window-based spatiotemporal calibration parameter optimization algorithm obtains the optimal solutions for the systematic time offset between the endoscope and the sensor, and the external transfer matrix (i.e., the relative pose relationship between the endoscope lens and the magnetic sensor). Specifically, the specific steps of the proposed sliding window-based spatiotemporal calibration parameter optimization algorithm are as follows:

[0088] Step 5.1: Based on the preliminary estimates of the external transfer matrix between sensors and the scaling coefficients between trajectories output in Step 4, set the time offset interval [t1 t] n The offset step size is t, ranging from -1.5s to 1.5s. step For 0.1s, iterate through different time offset values ​​and compensate to the timestamp corresponding to the magnetic positioning information obtained in step 1. Repeat steps 3 and 4 to record the scaling factor between the newly obtained external transfer matrix and the trajectory.

[0089] Step 5.2: Calculate the timestamp compensation for different time offsets t in the magnetic positioning information from Step 5.1. i The scaling factor s obtained at that time i Find the maximum value s among the scaling factors within the interval. max ,like Figure 9 As shown. Set the maximum value s in the scaling factor. max The corresponding external transition matrix is ​​the currently estimated optimal scaling factor and external transition matrix;

[0090] Step 5.3: Set the parameter accuracy threshold to 0.01, and repeat steps 5.1 and 5.2 until the changes in each parameter and scaling factor in the external matrix are less than the set threshold of 0.01. The resulting systematic time offset between the endoscope and sensor is 0.4s, and the final external translation matrix is:

[0091] [-0.01924939 0.023014160-0.050470593]

[0092] The external rotation matrix is:

[0093]

[0094] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for spatio-temporal co-calibration of magnetic sensors and endoscopes, characterized in that: The method comprises the following steps: Step 1 : Acquire endoscope images and magnetic localization information by manipulating an endoscope equipped with magnetic sensors to view a calibration board or other object of known specific dimensions, and record the timestamps of the magnetic sensors in real time and the timestamps of the images ; Step 2: Obtain the internal and external parameters and distortion parameters of the endoscope image based on a feature point-based camera calibration method; obtain the external parameter trajectory of the endoscope lens; Step 3: with image timestamp Interpolate the magnetic localization information with the target timestamp to synchronize the timestamps of the image and the magnetic localization information; obtain a magnetic localization trajectory; Step 4: input the endoscope lens extrinsic parameter trajectory obtained in step 2 and the magnetic positioning trajectory obtained in step 3, and output the preliminary estimated external transfer matrix between the endoscope and the magnetic sensor through the hand-eye calibration method with unknown translation scale and the scale scaling coefficient between the trajectories ; Step 5: Obtain the systematic time offset of the endoscope and the sensor and the external transfer matrix, i.e. the optimal solution of the relative pose relationship between the endoscope lens and the magnetic sensor, by using a sliding window-based spatio-temporal calibration parameter optimization algorithm.

2. The magnetic sensor and endoscope spatiotemporal co-calibration method of claim 1, wherein, Timestamp of the magnetic sensor in step 1 Timestamp of the image The multi-sensor data is not synchronized in time, and the timestamps are not synchronized with real time.

3. The magnetic sensor and endoscope spatiotemporal co-calibration method of claim 1, wherein, The specific steps of step 2 are as follows: Step 2.1: Input the endoscope image, obtain the feature point matching pairs by a feature extraction and matching algorithm, and record the pixel coordinates of the feature points in the matching pairs; Step 2.2: Calculate the physical coordinates of each feature point in the world coordinate system by using the size information of a calibration board or other known objects; Step 2.3: Input the pixel coordinates and physical coordinates of the feature points into a camera imaging model to solve the internal and external parameters of the camera, and estimate the distortion parameters by a least square method.

4. The magnetic sensor and endoscope spatiotemporal co-calibration method of claim 1, wherein, The specific steps of step 3 are as follows: Step 3.1: Interpolate the position information of the magnetic positioning by a position interpolation extrapolation method to synchronize the timestamps of the image and the position information; Step 3.2: Interpolate the attitude information of the magnetic positioning by a spherical linear interpolation method to synchronize the timestamps of the image and the attitude information of the magnetic sensor.

5. The method of claim 1, wherein, The specific steps of step 5 are as follows: Step 5.1: The algorithm sets the time offset interval based on the preliminary estimated external transfer matrix between sensors and the scale factor between trajectories and the offset step to traverse different time offset values to compensate for the time stamp corresponding to the collected magnetic positioning information; Step 5.2: Compensate different time offsets for statistical magnetic positioning information timestamps a scale factor obtained at time t , obtain the maximum value in the scale factor in the interval , set and its corresponding external transfer matrix as the current estimated best scale factor and external transfer matrix Step 5.3: Set a given parameter precision threshold, and repeat the iteration after updating the best scaling coefficient and the external transfer matrix until the changes of the parameters in the external matrix and the scaling coefficient are less than the set threshold, which is taken as the final systematic time offset of the endoscope and the sensor and the external transfer matrix, i.e. the relative pose relationship between the endoscope lens and the magnetic sensor.

Images