CN101099673A - Surgical instrument positioning method using infrared reflecting ball as symbolic point - Google Patents

Abstract

The present invention relates to an operative instrument positioning method by adopting infrared reflecting ball as marker point. Said method includes the following steps: respectively defining internal parameter and first order radial distortion parameter of left and right cameras; calculating rotation matrix and translation vector characterizing relative position relationship of left and right two cameras; utilizing infrared light source to irradiate guide frame in which three infrared reflecting balls are mounted; shooting to obtain left and right two images containing elliptical images of infrared reflecting balls and making distortion correction; combining elliptical images of left and right image planes which are undergone the process of distortion correction and directly and linearly calculating projection points of three infrared reflecting ball cores; matching said projection points and reconstructing ball core coordinates of three infrared reflecting balls; finally utilizing coordinate conversion to obtain the coordinate of tip end working portion of operative instrument.

Description

Adopting infrared reflecting ball is the surgical instrument positioning method of index point

Technical field

The present invention relates to a kind of infrared reflecting ball that adopts is the surgical instrument positioning method of index point, can be applicable to the operating theater instruments tracing-positioning system in the surgical navigational instrument, and can expanding to all, to adopt infrared reflecting balls be in the optical positioning system of index point.The invention belongs to advanced medical equipment and technical field.

Background technology

Up to the present, surgical operation mainly relies on the doctor with the naked eye to judge the position of focus, thereby the excision that undergos surgery waits operation.The shortcoming of this mode is that surgical openings is big, and is hemorrhage many, and the post-operative recovery time is long.Along with the development of computer generated image technology, virtual reality technology and theory on computer vision, operation guiding system (Surgical Navigation System) more and more is subject to people's attention.Operation guiding system can provide directly perceived, a visual operation technique platform to the doctor, and Minimally Invasive Surgery is become a reality, and patient's misery alleviates greatly.At western developed country, Minimally Invasive Surgery even remote operation under operation guiding system is assisted a ruler in governing a country are promoted gradually.

In order to realize surgical navigational, operating theater instruments is followed the tracks of accurately and located is indispensable step.The navigation position instrument that Canada NDI company produces is the main product on the present international market.But because the defective of existing location algorithm, the efficient working range of nearly all navigator fix equipment is all narrower, often can't satisfy the requirement of any mobile operating theater instruments in the operation process.At present, the elliptic projection center of people's infrared reflecting sign ball commonly used replaces the subpoint of the witch ball centre of sphere, but the only approximate establishment when operating theater instruments is in the camera field of view zone line of this prerequisite.But, in operation process, probably need make comparisons and move on a large scale operating theater instruments.In case operating theater instruments departs from the central area of camera field of view, positioning accuracy will reduce, and so probably causes risk operation even malpractice.At present, common navigation product exceeds the scope of application by the prompting user and avoids risk to take place, but this often causes use inconvenience.Such as, NDI company just uses its product strict regulation (POLARIS User Manual, Http:// www.ndigital.com).Simultaneously, prior art requires to reduce the diameter of witch ball as far as possible, but hot spot is when very little, and the hot spot border just is subjected to various interference of noise on the image easily, thereby produces bigger error.

In a word,, need accurately determine the gnomonic projection point of infrared reflecting ball, and then the space coordinates of definite witch ball centre of sphere, rather than replace the subpoint of the centre of sphere with the center of the oval image of witch ball in order to enlarge the efficient working range of navigator.Therefore, a kind of can have higher utility with the algorithm that less calculation cost is asked for witch ball gnomonic projection point.

Summary of the invention

The objective of the invention is at the deficiencies in the prior art, proposing a kind of infrared reflecting ball that adopts is the surgical instrument positioning method of index point, enlarges the efficient working range of operation guiding system, improves the positioning accuracy of operating theater instruments.

Above-mentioned purpose of the present invention is achieved through the following technical solutions: utilize single camera calibration method, determine the intrinsic parameter and the single order radial distortion parameter of left and right cameras respectively, utilize the stereo camera scaling method, calculate the spin matrix and the translation vector that characterize the left and right cameras relative position relation; With infrared light supply irradiation 3 not operating theater instruments navigation supports of the infrared reflecting ball on same straight line are installed, utilize left and right cameras to take the navigation support simultaneously again, thereby two width of cloth comprise the image of the oval image of 3 witch balls about obtaining; This two width of cloth image is carried out distortion correction, make it not contain distortion information; Through detecting the border of the oval image of each witch ball on the left and right sides image of distortion correction respectively, utilize the boundary pixel point to simulate elliptic equation, thereby on the image of the left and right sides, obtain the mathematical notation matrix of 3 oval images respectively; Obtain the antithesis matrix of each matrix respectively, utilize corresponding restriction relation to calculate the position of witch ball gnomonic projection point; 3 gnomonic projection points that left and right sides image is obtained respectively carry out a coupling, and it is right to obtain 3 points, utilizes them to reconstruct the space coordinates of 3 witch ball centre ofs sphere again; Carry out a series of coordinate transform at last and can calculate the dimensional orientation of the most advanced and sophisticated working position of operating theater instruments.

Localization method involved in the present invention comprises following concrete steps:

(1) camera calibration: left and right cameras is utilized single camera calibration method respectively, determine the intrinsic parameter matrix A of left video camera _l, left video camera single order radial distortion parameter S _l, right video camera the intrinsic parameter matrix A _r, right video camera single order radial distortion parameter S _rUtilize the stereo camera scaling method, calculate the spin matrix R that characterizes the left and right cameras relative position relation ₀With translation vector t ₀

(2) near distortion correction: with infrared light supply irradiation the not navigation support of three infrared reflecting balls on same straight line is installed, and left and right cameras is placed on the navigation support, guarantee that this navigation support is in the public view field scope of left and right cameras; Utilize left and right cameras to take the navigation support simultaneously, thereby obtain the image that a width of cloth comprises the oval image of three infrared reflecting balls, simultaneously, also obtain the image that a width of cloth comprises the oval image of three infrared reflecting balls at right video camera at left video camera; Utilize the single order radial distortion parameter S of left video camera _lLeft image is carried out distortion correction, do not contained the left image of distortion information; Simultaneously, utilize the single order radial distortion parameter S of right video camera _rRight image is carried out distortion correction, do not contained the right image of distortion information;

(3) border detection and ellipse fitting: utilize the canny operator, detect the border of the oval image of witch ball on two width of cloth planes of delineation behind the distortion correction respectively, thereby it is right to obtain the point that 3 groups of coordinates by oval image boundary pixel constitute on the image of the right and left respectively; Utilize 3 groups of points of left image right respectively, simulate the Matrix C of three the oval images in the expression left side _L1, C _L2, C _L3, utilize 3 groups of points of right image right more respectively, simulate the Matrix C of three the oval images in expression the right _R1, C _R2, C _R3

(4) calculate the gnomonic projection point: calculate C respectively _LiThe antithesis Matrix C _Li ^*, C _RiThe antithesis Matrix C _Ri ^*, i=1 wherein, 2,3;

Utilize following formula:

$k_{\mathrm{li}}{C}_{\mathrm{li}}^{*}=A_l{A_l}^T-v_{\mathrm{li}}-{v_{\mathrm{li}}}^T$

$k_{\mathrm{ri}}{C}_{\mathrm{ri}}^{*}=A_r{A_r}^T-v_{\mathrm{ri}}{v_{\mathrm{ri}}}^T,$

Solve k _Li, k _Ri, v _Li, v _RiK wherein _Li, k _RiBe scale factor, i=1,2,3; Three dimensional vector v _LiExpression and oval C _LiThe homogeneous coordinates of the subpoint on the corresponding infrared reflecting ball centre of sphere on the left side image, three dimensional vector v _RiExpression and oval C _RiThe corresponding infrared reflecting ball centre of sphere is the homogeneous coordinates of the subpoint on the image on the right; Thereby on the image of the left and right sides, obtain the homogeneous coordinates of the subpoint of three infrared reflecting ball centre ofs sphere respectively;

(5) coupling and rebuild: three gnomonic projection points on the left image and three gnomonic projection points on the right image are carried out a coupling, obtain 3 groups of corresponding point; Utilize the intrinsic parameter matrix A of left and right cameras _l, A _rAnd spin matrix R ₀With translation vector t ₀Carry out a reconstruction, obtain three space coordinatess under the infrared reflecting ball centre of sphere on the left side camera coordinate system;

(6) Coordinate Conversion: when the navigation support is installed on operating theater instruments, the centre of sphere line of choosing 2 infrared reflecting balls arbitrarily is as X-axis, the centre of sphere of the 3rd infrared reflecting ball is a Y-axis to the vertical line of X-axis, the point that hangs down is zero, set up navigation support coordinate system according to right-hand rule, determine the coordinate of the most advanced and sophisticated working position of operating theater instruments under this coordinate system simultaneously; Space coordinates under three infrared reflecting ball centre of sphere on the left side camera coordinate systems that utilization obtains calculates spin matrix R and translation vector t between navigation support coordinate system and the left side camera coordinate system; Utilize spin matrix R and translation vector t, calculate the coordinate of the most advanced and sophisticated working position of operating theater instruments under left camera coordinate system, finish location thus operating theater instruments.

Compare with existing method, the present invention has accurately determined the subpoint of the infrared reflecting ball centre of sphere, makes that the positioning accuracy of location of operation apparatus is not different along with the position change of operating theater instruments.Simultaneously, the efficient working range of operating theater instruments enlarges, and the motion limits of operation technique is greatly reduced.The present invention can be used to improve existing navigator, and need not to change hardware setting, helps the transition upgrading of existing product.

Description of drawings

Fig. 1 is the sketch map of the surgical instrument positioning method of index point for the present invention adopts infrared reflecting ball.

Fig. 2 is the foundation and the Coordinate Conversion sketch map of navigation support coordinate system.

The specific embodiment

In order to understand technical scheme of the present invention better, embodiments of the present invention are described in detail below in conjunction with drawings and Examples.

Fig. 1 is the sketch map of localization method of the present invention, wherein is depicted as a binocular tri-dimensional vision system that disposes arbitrarily.O _lX _lY _lZ _lAnd O _rX _rY _rZ _rBe respectively the left and right cameras coordinate system, o _lu _lv _lAnd o _ru _rv _rBeing respectively with the pixel is the left and right sides image coordinate system of unit.Be equipped with 3 not the navigation support of the infrared reflecting ball on same straight line be placed in the public view field scope of left and right cameras.Take the navigation support simultaneously with left and right cameras, obtain 3 ellipses this moment respectively on the image of the left and right sides, and they are 3 infrared reflecting ball projections on image.

Describe the implementation step of the inventive method below in detail:

1. to left and right cameras, utilize single camera calibration method (Z.Zhang respectively, Flexible cameracalibration by viewing a plane from unknown orientations, proceedingsof the Fifth International Conference on Computer Vision, 1999, pp.666-673), determine the intrinsic parameter matrix A of left side video camera _l, left side video camera single order radial distortion parameter S _l, the right video camera the intrinsic parameter matrix A _r, the right video camera single order radial distortion parameter S _rUtilize stereo camera scaling method (Jean-Yves Bouguet, Camera Calibration Toolbox forMatlab, MRL-Intel Incorp.), calculate the spin matrix R that characterizes the left and right cameras relative position relation ₀With translation vector t ₀Wherein, the form of intrinsic parameter matrix is

$A_l=\left[\begin{array}{ccc}{\mathrm{\α}}_l& {\mathrm{\γ}}_l& u_{0l}\\ 0& {\mathrm{\β}}_l& v_{0l}\\ 0& 0& 1\end{array}\right],$ $A_r=\left[\begin{array}{ccc}{\mathrm{\α}}_r& {\mathrm{\γ}}_r& u_{0r}\\ 0& {\mathrm{\β}}_r& v_{0r}\\ 0& 0& 1\end{array}\right].$

2. with infrared light supply irradiation the not navigation support of three infrared reflecting balls on same straight line is installed, and left and right cameras is placed near the navigation support, guarantee that this navigation support is in the public view field scope of left and right cameras; Utilize left and right cameras to take the navigation support simultaneously, thereby obtain the image that a width of cloth comprises the oval image of three infrared reflecting balls, simultaneously, also obtain the image that a width of cloth comprises the oval image of three infrared reflecting balls at right video camera at left video camera; Utilize the single order radial distortion parameter S of left video camera _lLeft image is carried out distortion correction, do not contained the left image of distortion information, be designated as π _lSimultaneously, utilize the single order radial distortion parameter S of right video camera _rRight image is carried out distortion correction, do not contained the right image of distortion information, be designated as π _r

Concrete trimming process is: to the image on the left side, establish the coordinate of certain picture point that comprises distortion information under the image coordinate system that with the pixel is unit and be

Its normalized image coordinate is

The picture point that does not contain distortion information of their correspondences be designated as respectively (u, v) and (x, y).According to document (D.C.Brown, Close-range camera calibration, Photogram-metric Engineering, 37 (8): 855-866,1971), have

$\hat{x}=x+x\left[S_l(x^2+y^2)\right]$

$\hat{y}=y+y\left[S_l(x^2+y^2)\right]$

Utilize the coordinate transform formula

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=A_l\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{\α}}_l& {\mathrm{\γ}}_l& u_{0l}\\ 0& {\mathrm{\β}}_l& v_{0l}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right],$

$\left[\begin{array}{c}\hat{u}\\ \hat{v}\\ 1\end{array}\right]=A_l\left[\begin{array}{c}\hat{x}\\ \hat{y}\\ 1\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{\α}}_l& {\mathrm{\γ}}_l& u_{0l}\\ 0& {\mathrm{\β}}_l& v_{0l}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\hat{x}\\ \hat{y}\\ 1\end{array}\right],$ S wherein _lBe left side video camera single order radial distortion parameter, A _lBe the intrinsic parameter matrix of left video camera, and all determine by demarcating.

Can obtain

$\hat{u}=u+(u-u_{0l})\left[S_l(x^2+y^2)\right]$

$\hat{v}=v+(v-v_{0l})\left[S_l(x^2+y^2)\right]$

Because top equation is a Nonlinear System of Equations, in order to simplify solution procedure, above-mentioned equation group can be approximately (Janne Heikkila, Geometric Camera Calibration Using Circular ControlPoints, IEEE.0n Pattern Analysis and Machine Intelligence, vol.22, no.10,1066-1077)

$u=\hat{u}-(\hat{u}-u_{0l})\left[S_l({\hat{x}}^2+{\hat{y}}^2)\right]$

$v=\hat{v}-(\hat{v}-v_{0l})\left[S_l({\hat{x}}^2+{\hat{y}}^2)\right]$

Two formulas can be carried out distortion correction to each picture point on the left image above utilizing, thereby are not contained the image π of distortion information _lFor the image on the right, the bearing calibration of bearing calibration and left image is identical.

3. utilize the canny operator, detect π respectively _lAnd π _rThe border of the oval image of infrared reflecting ball on two width of cloth planes of delineation, thus it is right to obtain the point that 3 groups of coordinates by oval image boundary pixel constitute on the image of the right and left respectively; Utilize 3 groups of points of left image right respectively, simulate the Matrix C of three the oval images in the expression left side _L1, C _L2, C _L3, utilize 3 groups of points of right image right more respectively, simulate the Matrix C of three the oval images in expression the right _R1, C _R2, C _R3Concrete method is referring to (A.W.Fitzgibbon, M.Pilu, and R.B.Fisher, " Direct Least-Squares Fitting of Ellipses ", IEEE Trans.Pattern Analysis and Machine Intelligence, vol.14, no.2, pp.239-256).

4. calculate C respectively _LiThe antithesis Matrix C _Li ^*, C _RiThe antithesis Matrix C _Ri ^*, i=1 wherein, 2,3;

Utilize following formula:

$k_{\mathrm{li}}{C}_{\mathrm{li}}^{*}=A_l{A_l}^T-v_{\mathrm{li}}{v_{\mathrm{li}}}^T$

$k_{\mathrm{ri}}{C}_{\mathrm{ri}}^{*}=A_r{A_r}^T-v_{\mathrm{ri}}{v_{\mathrm{ri}}}^T,$

Solve k _Li, k _Ri, v _Li, v _RiK wherein _Li, k _RiBe scale factor, i=1,2,3; Three dimensional vector v _LiExpression and oval C _LiThe homogeneous coordinates of the subpoint on the corresponding infrared reflecting ball centre of sphere on the left side image, three dimensional vector v _RiExpression and oval C _RiThe corresponding infrared reflecting ball centre of sphere is the homogeneous coordinates of the subpoint on the image on the right; Thereby on the image of the left and right sides, obtain the homogeneous coordinates of the subpoint of three infrared reflecting ball centre ofs sphere respectively;

Concrete grammar is: compute matrix A _lA _l ^TAnd Matrix C _Li ^*Generalized eigenvalue, then unknown scale factor k _LiBe their double generalized eigenvalue; To A _lA _l ^T-k _LiC _Li ^*Carry out eigenvalue and decompose, establish its nonzero eigenvalue characteristic of correspondence vector and be β _i, so $v_{\mathrm{li}}=\frac{{\mathrm{\β}}_i}{\left|\right|{\mathrm{\β}}_i\left|\right|},$ Thereby calculate and oval C _LiThe homogeneous coordinates of the subpoint on the corresponding infrared reflecting ball centre of sphere on the left side image; Image to the right adopts identical method to handle.

5. coupling and rebuild: three gnomonic projection points on the left image and three gnomonic projection points on the right image are carried out a coupling, obtain 3 groups of corresponding point; Utilize the intrinsic parameter matrix A of left and right cameras _l, A _rAnd spin matrix R ₀With translation vector t ₀Carry out a reconstruction, obtain three space coordinatess under the infrared reflecting ball centre of sphere on the left side camera coordinate system;

Concrete grammar is: because the position of left and right cameras is nearer apart, and is in the same side of navigation support, the position of 3 gnomonic projection points on the image of the left and right sides has similarity so.Scan line by line on the plane of delineation of the left and right sides respectively, the left and right sides gnomonic projection point that directly will be in similar position mates.

Utilize the spatial point method for reconstructing, in the image coordinate of known two subpoints and the intrinsic parameter matrix A of left and right cameras _l, A _rAnd the spin matrix R of expression left and right cameras relative position relation ₀With translation vector t ₀The time, can reconstruct the space coordinates (Ma Songde, Zhang Zhengyou, the computer vision---theory of computation and algorithm basis, Science Press, 1999) of the centre of sphere under left camera coordinate system.

When (6) the navigation support being installed on operating theater instruments, the centre of sphere line of choosing 2 infrared reflecting balls arbitrarily is as X-axis, and the centre of sphere of the 3rd infrared reflecting ball is a Y-axis to the vertical line of X-axis, and the point that hangs down is zero, sets up navigation support coordinate system according to right-hand rule; Determine the coordinate of the most advanced and sophisticated working position of operating theater instruments under this coordinate system simultaneously; Space coordinates under three infrared reflecting ball centre of sphere on the left side camera coordinate systems that utilization obtains calculates spin matrix R and translation vector t between navigation support coordinate system and the left side camera coordinate system; Utilize spin matrix R and translation vector t, calculate the coordinate of the most advanced and sophisticated working position of operating theater instruments under left camera coordinate system, finish location thus operating theater instruments.

Figure 2 shows that navigation support coordinate system is set up and the sketch map of coordinate transform.Might as well select centre of sphere O _AAnd O _BLine be X-axis, from centre of sphere O _CMake the vertical line of X-axis, as zero, be designated as O with the point that hangs down _w,, set up navigation support coordinate system O according to right-hand rule _w-X _wY _wZ _wBy accurate installation, determine the most advanced and sophisticated working position O of operating theater instruments _DCoordinate under navigation support coordinate system is designated as α _D=[X _WDY _WDZ _WD] ^TIf centre of sphere O _A, O _B, O _CCoordinate under left camera coordinate system is respectively α _A=[X _CAY _CAZ _CA] ^T, α _B=[X _CBY _CBZ _CB] ^T, α _C=[X _CCY _CCZ _CC] ^TNavigation support coordinate system O _w-X _wY _wZ _wWith left camera coordinate system O _l-X _lY _lZ _lBetween spin matrix be that R, translation vector are t; Zero O then _wCoordinate under left camera coordinate system is ${\mathrm{\α}}_O=\left[\begin{array}{c}{X}_{\mathrm{cO}}\\ Y_{\mathrm{cO}}\\ Z_{\mathrm{cO}}\end{array}\right]=\left[\begin{array}{c}{X}_{\mathrm{cA}}\\ Y_{\mathrm{cA}}\\ Z_{\mathrm{cA}}\end{array}\right]-\mathrm{\λ}(\left[\begin{array}{c}{X}_{\mathrm{cB}}\\ Y_{\mathrm{cB}}\\ Z_{\mathrm{cB}}\end{array}\right]-\left[\begin{array}{c}{X}_{\mathrm{cA}}\\ Y_{\mathrm{cA}}\\ Z_{\mathrm{cA}}\end{array}\right]),$ Wherein

$\mathrm{\λ}=\frac{(X_{\mathrm{cB}}-X_{\mathrm{cA}})(X_{\mathrm{cC}}-X_{\mathrm{cA}})+(Y_{\mathrm{cB}}-Y_{\mathrm{cA}})(Y_{\mathrm{cC}}-Y_{\mathrm{cA}})+(Z_{\mathrm{cB}}-Z_{\mathrm{cA}})(Z_{\mathrm{cC}}-Z_{\mathrm{cA}})}{{(X_{\mathrm{cB}}-x_{\mathrm{cA}})}^2+{(Y_{\mathrm{cB}}-Y_{\mathrm{cA}})}^2+{(Z_{\mathrm{cB}}-Z_{\mathrm{cA}})}^2}$

Spin matrix then $R=\left[\begin{array}{ccc}\frac{{\mathrm{\α}}_A-{\mathrm{\α}}_O}{\left|\right|{\mathrm{\α}}_A-{\mathrm{\α}}_O\left|\right|}& \frac{{\mathrm{\α}}_C-{\mathrm{\α}}_O}{\left|\right|{\mathrm{\α}}_C-{\mathrm{\α}}_O\left|\right|}& \frac{{\mathrm{\α}}_A-{\mathrm{\α}}_O}{\left|\right|{\mathrm{\α}}_A-{\mathrm{\α}}_O\left|\right|}\×\frac{{\mathrm{\α}}_C-{\mathrm{\α}}_O}{\left|\right|{\mathrm{\α}}_C-{\mathrm{\α}}_O\left|\right|}\end{array}\right]$

Translation vector t=α _A-λ (α _B-α _A)

The most advanced and sophisticated working position of operating theater instruments is at left camera coordinate system O _l-X _lY _lZ _lUnder coordinate be R α _D+ t, so far, finish the operating theater instruments location.

Claims (1)

1, a kind of infrared reflecting ball that adopts is the surgical instrument positioning method of index point, it is characterized in that comprising following steps:

(1) camera calibration: left and right cameras is utilized single camera calibration method respectively, determine the intrinsic parameter matrix A of left video camera _l, left video camera single order radial distortion parameter S _l, right video camera the intrinsic parameter matrix A _r, right video camera single order radial distortion parameter S _rUtilize the stereo camera scaling method, calculate the spin matrix R that characterizes the left and right cameras relative position relation ₀With translation vector t ₀

(2) near distortion correction: with infrared light supply irradiation the not navigation support of three infrared reflecting balls on same straight line is installed, and left and right cameras is placed on the navigation support, guarantee that this navigation support is in the public view field scope of left and right cameras; Utilize left and right cameras to take the navigation support simultaneously, thereby obtain the image that a width of cloth comprises the oval image of three infrared reflecting balls, simultaneously, also obtain the image that a width of cloth comprises the oval image of three infrared reflecting balls at right video camera at left video camera; Utilize the single order radial distortion parameter S of left video camera _lLeft image is carried out distortion correction, do not contained the left image of distortion information; Simultaneously, utilize the single order radial distortion parameter S of right video camera _rRight image is carried out distortion correction, do not contained the right image of distortion information;

(3) border detection and ellipse fitting: utilize the canny operator, detect the border of the oval image of witch ball on two width of cloth planes of delineation behind the distortion correction respectively, thereby it is right to obtain the point that 3 groups of coordinates by oval image boundary pixel constitute on the image of the right and left respectively; Utilize 3 groups of points of left image right respectively, simulate the Matrix C of three the oval images in the expression left side _L1, C _L2, C _L3, utilize 3 groups of points of right image right more respectively, simulate the Matrix C of three the oval images in expression the right _R1, C _R2, C _R3

(4) calculate the gnomonic projection point: calculate C respectively _LiThe antithesis Matrix C _Li ^*, C _RiThe antithesis Matrix C _Ri ^*, i=1 wherein, 2,3;

Utilize following formula:

$k_{\mathrm{li}}{C}_{\mathrm{li}}^{*}=A_l{A_l}^T-v_{\mathrm{li}}{v_{\mathrm{li}}}^T$

$k_{\mathrm{ri}}{C}_{\mathrm{ri}}^{*}=A_r{A_r}^T-v_{\mathrm{ri}}{v_{\mathrm{ri}}}^T,$

Solve k _Li, k _Ri, v _Li, v _RiK wherein _Li, k _RiBe scale factor, i=1,2,3; Three dimensional vector v _LiExpression and oval C _LiThe homogeneous coordinates of the subpoint on the corresponding infrared reflecting ball centre of sphere on the left side image, three dimensional vector v _RiExpression and oval C _RiThe corresponding infrared reflecting ball centre of sphere is the homogeneous coordinates of the subpoint on the image on the right; Thereby on the image of the left and right sides, obtain the homogeneous coordinates of the subpoint of three infrared reflecting ball centre ofs sphere respectively;

(5) coupling and rebuild: three gnomonic projection points on the left image and three gnomonic projection points on the right image are carried out a coupling, obtain 3 groups of corresponding point; Utilize the intrinsic parameter matrix A of left and right cameras _l, A _rAnd spin matrix R ₀With translation vector t ₀Carry out a reconstruction, obtain three space coordinatess under the infrared reflecting ball centre of sphere on the left side camera coordinate system;

(6) Coordinate Conversion: when the navigation support is installed on operating theater instruments, the centre of sphere line of choosing 2 infrared reflecting balls arbitrarily is as X-axis, the centre of sphere of the 3rd infrared reflecting ball is a Y-axis to the vertical line of X-axis, the point that hangs down is zero, set up navigation support coordinate system according to right-hand rule, determine the coordinate of the most advanced and sophisticated working position of operating theater instruments under this coordinate system simultaneously; Space coordinates under three infrared reflecting ball centre of sphere on the left side camera coordinate systems that utilization obtains calculates spin matrix R and translation vector t between navigation support coordinate system and the left side camera coordinate system; Utilize spin matrix R and translation vector t, calculate the coordinate of the most advanced and sophisticated working position of operating theater instruments under left camera coordinate system, finish location thus operating theater instruments.

Images