CN103925919A - Fisheye camera based planetary rover detection point positioning method - Google Patents

Abstract

The invention discloses a fisheye camera based planetary rover detection point positioning method. The method comprises the following steps: transforming a fisheye imaging projection to a center projection; generating an image epipolar image; extracting and coupling an imaging characteristic point; and calculating the coordinates of the detection point. The high precision positioning of the planetary rover detection point can be realized through the method by using a fisheye camera.

Description

A kind of planet car sensing point localization method based on fisheye camera

Technical field

The present invention relates to computer vision and technical field of image matching, is a kind of localization method that utilizes fisheye camera image to carry out planet car sensing point.

Background technology

At present, abroad, in planetary detection, can realize planetary surface navigation and the detection of a target are located by the two CCD camera measure system of installing on planet car.U.S. NASA " curious number " Marsokhod in survey of deep space task is collected sample, diamond drill hole sampling by mechanical arm, and wherein the main barrier camera of keeping away carrying that utilizes obtains the three-dimensional coordinate that stereo-picture obtains sensing point position.Domesticly for mechanical arm Detection location, also carry out a series of research, but mainly concentrate on, used the less stereoscopic camera vision system of field angle to carry out sensing point location.The advantage of fisheye camera is to have large field angle, once can see interior in a big way scene.But fisheye camera exists larger distortion, how carrying out high-precision location is its difficult point that need to solve.

Summary of the invention

The technical problem to be solved in the present invention is, the present invention proposes a kind of planet car sensing point localization method based on fisheye camera, makes can realize by fisheye camera the hi-Fix of planet car sensing point.

For addressing the above problem, the present invention adopts following technical scheme:

A kind of planet car sensing point localization method based on fisheye camera comprises the following steps:

S1, left and right fisheye camera original image is converted to respectively to the central projection image under central projection pattern;

S2, central projection image is carried out to core line correct to process, be converted to left and right core line image;

S3, from left core line image extract minutiae, on right core line image, according to described unique point, utilize point that correlation coefficient process therefrom searches out related coefficient maximum as match point, then utilize least square method, the match point that correlation coefficient process is extracted carries out compensating computation, makes the matching precision of left and right core line image reach sub-pixel grade.

S4, the high precision matching characteristic point obtaining according to least squares adjustment, utilize forward intersection algorithm to calculate sensing point coordinate.

The present invention adopts perspective projection transformation, and fish eye images is converted to central projection image, and produced nucleus line image, and coupling is narrowed down in core line direction, has reduced without coupling; Adopt the first strategy of the rear exact matching of thick coupling, first realize the coupling of whole Pixel-level and then carry out the exact matching that least square coupling realizes sub-pixel precision, guaranteed the high precision of coupling, the accurate location of finally having realized sensing point.Adopt technical scheme of the present invention, make can realize by fisheye camera the hi-Fix of planet car sensing point.

Accompanying drawing explanation

Fig. 1 is the planet car sensing point localization method process flow diagram based on fisheye camera;

Fig. 2 is that flake projection is to central Projection Transformation And schematic diagram;

Fig. 3 is produced nucleus line image schematic diagram.

Embodiment

The invention provides a kind of planet car sensing point localization method based on fisheye camera comprises:

S1, flake projection are to central Projection Transformation And: left and right fisheye camera original image is converted to respectively to the central projection image under central projection pattern.

Arbitrary spatial point P is converted to central projection pattern hypograph coordinate (x ', y ') at the picture coordinate (x, y) of fisheye camera imaging; As shown in Figure 2, O ' is optical center, O ' X ' Y ' plane is that fish eye lens is as plane, f is the focal length of fisheye camera, and O ' Z is primary optical axis, and the O ' of take sets up image space coordinate system O '-X ' Y ' Z as coordinate origin, arbitrary spatial point P, under image space coordinate system corresponding to fisheye camera coordinate be (x ', y ', z).O ' P line and the projection sphere p that intersects at a point, the orthogonal projection of p point to O ' X ' Y ' as in plane, obtain picture point p ', coordinate is (x, y), (x, y) be a P under image space coordinate system corresponding to fisheye camera coordinate (x ', y ', spherical projection z), spherical equation is x ^{' 2}+ y ^{' 2}+ z ²=f ².Supposing has a virtual picture plane OXY at the plane place of Z=f, corresponding change in coordinate axis direction is parallel with the middle coordinate axis of picture plane O ' X ' Y ', and O ' P line and its intersection point are p ".Obviously p " for the virtual central projection picture point of some P, this p " is shown (x ', y ') as coordinates table under the central projection of point.P, p " and 3 collinearity equations of following central projection of O ', between each coordinate, there is following relation, see formula (2):

$\left\{\begin{array}{c}{x}^{\′}=\frac{f\·x}{\sqrt{f^2-x^2-y^2}}\\ y^{\′}=\frac{f\·y}{\sqrt{f^2-x^2-y^2}}\end{array}\right.---\left(1\right)$

Making the picpointed coordinate of the left image of original fisheye camera is (x _l, y _l), the picpointed coordinate of the right image of original fisheye camera is (x _r, y _r), according to formula (1), have the picpointed coordinate of their correspondences on central projection image be respectively (x ' _l, y ' _l), (x ' _r, y ' _r), their relation is shown in formula (2), (3):

$\left\{\begin{array}{c}{x}_L^{\′}=\frac{f\·x_L}{\sqrt{f^2-x_L^2-y_L^2}}\\ y_L^{\′}=\frac{f\·y_R}{\sqrt{f^2-x_L^2-y_L^2}}\end{array}\right.---\left(2\right)$ $\left\{\begin{array}{c}{x}_R^{\′}=\frac{f\·x_R}{\sqrt{f^2-x_L^2-y_L^2}}\\ y_R^{\′}=\frac{f\·y_R}{\sqrt{f^2-x_R^2-y_R^2}}\end{array}\right.---\left(3\right)$

S2, synthetic image core line image: central projection image is carried out to core line and correct processing, be converted to left and right core line image.

If the picpointed coordinate of picture point on left and right core line image be respectively (x " _l, y " _l), (x " _r, y " _r), the picpointed coordinate on they and central projection image (x ' _l, y ' _l), (x ' _r, y ' _r) transformational relation can be expressed as formula (4), (5)

N _turnfor the transition matrix between left fisheye camera core line image and its original image, N ' _turnfor the transition matrix between right fisheye camera core line image and its original image, $\left(\begin{array}{c}{X}_0^{\′}\\ Y_0^{\′}\end{array}\right)$ For the translation parameters of changing between left fisheye camera core line image and its original image, $\left(\begin{array}{c}{X}_0^{\′\′}\\ Y_0^{\′\′}\end{array}\right)$ For the translation parameters of changing between right fisheye camera core line image and its original image;

Solve N _turn, $\left(\begin{array}{c}{X}_0^{\′}\\ Y_0^{\′}\end{array}\right),$ N ' _turnand $\left(\begin{array}{c}{X}_0^{\′\′}\\ Y_0^{\′\′}\end{array}\right)$ Process as follows:

As shown in Figure 3, P is space any point, O _l, O _rbe respectively the optical center of left and right camera, O _l-X _ly _lz _l, O _r-X _ry _rz _rbe respectively the image space coordinate system of left and right camera, the coordinate of P under two coordinate systems be respectively (x ' _l, y ' _l, z ' _l) and (x ' _r, y ' _r, z ' _r).S _land S _rfor left and right picture plane, o _l, o _rbe respectively the principal point of left and right photo, two focal lengths as plane are respectively f _l, f _r.O _l-x _ly _l, o _r-x _ry _rbe picture coordinate systems corresponding to two picture planes, o _lx _l, o _ly _laxle respectively with O _lx _l, O _ly _laxle is parallel, o _rx _r, o _ry _raxle respectively with O _rx _r, O _ry _raxle is parallel; P ' _land p ' _rbe respectively the picture point of P correspondence in left and right picture plane, coordinate be respectively (x ' _l, y ' _l), (x ' _r, y ' _r).At O _lo _ro _lin 3 determined planes, set up left camera virtual representation space coordinates O _l-XTZ, i.e. left camera core line image space coordinate system, O _lfor coordinate origin, O _lpoint to O _rthe direction positive dirction that is X-axis, the positive dirction that the normal direction of plane is Z axis, Y-axis is determined by X and Z axis.In like manner set up the virtual representation space coordinates O of right camera _r-X ' Y ' Z ', i.e. right camera core line image space coordinate system, it is by coordinate system O _l-XYZ moves to O _rproduce.Make the coordinate of P under two coordinate systems be respectively (x " _l, y " _l, z " _l), (x " _r, y " _r, z " _r); S ' _land S ' _rbe respectively virtual representation plane corresponding to above two virtual representation space coordinates, their focal lengths are f, and S ' _land S ' _rcoplanar.O ' _l, o ' _rbe respectively S " _land S " _rprincipal point, o ' _l-xy, o ' _r-xy is picture coordinate systems corresponding to two picture planes, o ' _lx, o ' _ly axle respectively with O _lx, O _ly-axis is parallel, o ' _rx, o ' _ry axle respectively with O _rx, O _ry-axis is parallel.The essence that core line image is corrected is exactly at S by two of left and right camera _land S _rupper imaging projects to O _l-XYZ, O _r-X ' Y ' Z ' is virtual representation planar S corresponding to virtual representation space coordinates " _land S " _ron.

Detailed process is as follows:

(1) the image space coordinate system O of right camera _r-X _ry _rz _rimage space coordinate system O with left camera _l-X _ly _lz _lcoordinate transformation relation can use formula (6) to represent, wherein, N is coordinate system O _r-X _ry _rz _rwith coordinate system O _l-X _ly _lz _lthe rotation matrix of conversion, (X ₀, Y ₀, Z ₀) represent O _l-X _ly _lz _linitial point is at O _r-X _ry _rz _rcoordinate.

$\left(\begin{array}{c}{x}_L^{\′}\\ y_L^{\′}\\ z_L^{\′}\end{array}\right)=N\left(\begin{array}{c}{x}_R^{\′}-X_0\\ y_R^{\′}-Y_0\\ z_R^{\′}-Z_0\end{array}\right)---\left(6\right)$

(2) utilize the formula (6) can calculation level O _rcoordinate under the image space coordinate system of left camera expression formula is:

$\left(\begin{array}{c}{x}_{L{O}_R}^{\′}\\ y_{L{O}_R}^{\′}\\ z_{L{O}_R}^{\′}\end{array}\right)=N\left(\begin{array}{c}-X_0\\ -Y_0\\ -Z_0\end{array}\right)---\left(7\right)$

(3) at the O of left camera virtual representation space coordinates _lon Z axis, finding unit length is 1 some P ₁, its coordinate under the image space coordinate system of left camera so can be obtained by following formula:

$\stackrel{\→}{O_L{P}_1}=\frac{\stackrel{\→}{O_L{O}_R}\×\stackrel{\→}{O_L{o}_L}}{|\stackrel{\→}{O_L{O}_R}\×\stackrel{\→}{O_L{o}_L}|}=x_{L{P}_1}^{\′}i+y_{L{P}_1}^{\′}j+z_{L{P}_1}^{\′}k---\left(8\right)$

(4) O so _l, O _r, P ₁3 coordinates under left camera virtual representation space coordinates be respectively (0,0,0), their coordinates under the image space coordinate system of left camera be respectively (0,0,0), according to common point transfer principle, can obtain coordinate system O _l-XYZ and O _l-X _ly _lz _lconversion parameter: X ' _0L, Y ' _0L, Z ' _0L, ω ', κ ', φ ', wherein, X ' _0L, Y ' _0L, Z ' _0Lfor translation parameters, ω ', κ ', φ ' is rotation parameter, by ω ', κ ', φ ' parameter can calculate rotation matrix N _core; Coordinate system O _l-XYZ and O _l-X _ly _lz _lthere is following transformational relation:

Wherein,

(5) utilize above-mentioned (9) formula will look like planar S _lpicture point p ' _lcoordinate under left camera image space coordinate system (x ' _l, y ' _l,-f _l) be converted to coordinate under left camera virtual representation space coordinates (x " _l, y " _l, z " _l), there is following relation:

By formula (10), can solve N in formula (4) _turn, value,

In like manner, utilize identical method can solve N ' _turnwith $\left(\begin{array}{c}{Y}_0^{\′\′}\\ Y_0^{\′\′}\end{array}\right).$

(6) because the coordinate calculating not necessarily drops on the position of whole pixel just, to (x ' _l, y ' _l) carry out bilinear interpolation gray resample, can obtain (x " _l, y " _l) gray-scale value, thereby generate left core line image, in like manner obtain right core line image.

S3, image characteristic point extract and coupling: extract minutiae from left core line image, on right core line image, according to described unique point, utilize point that correlation coefficient process therefrom searches out related coefficient maximum as match point, then utilize least square method, the match point that correlation coefficient process is extracted carries out compensating computation, makes the matching precision of left and right core line image reach sub-pixel grade.

Set of assigned points in left core line image on right core line image, mate corresponding point, form coupling point set specifically comprise the following steps:

Step 1: sensing point is specified

The needs that can survey according to task on the left core line image generating, manually specify one or more detections of a target, and their picture coordinate meters under core line image coordinate system are done form point set $P\{(x_T^{\″i},y_T^{\″i}),i=\mathrm{1,2},...,n\}.$

Step 2: sensing point coupling

After the unique point of utilizing step 1 to realize left width image is specified, further work is exactly on another piece image, to carry out the coupling of specified point.Specified the core line image of sensing point to be called benchmark image, an other core line image to be matched is called registering images.Conventionally baseline shorter (being about 100mm) and the relative position between the fisheye camera on planet car fixed, and can think rigid body, therefore left and right stereo-picture similarity and degree of overlapping are all larger, can adopt correlation coefficient matching method method.Concrete steps are as follows:

(1) choose successively benchmark image point set in each sensing point, and centered by this puts, choose the capable n of m row (m, n is odd number, conventionally makes m=n=11) image-region as target area.

(2) in order to search for match point on registering images, first estimate the approximate range that this match point may exist, set up the field of search (k > m, 1 > n) of the capable l row of k.

(3) in the field of search of registering images, take out successively m * n pixel grey scale array (search window is got m=n conventionally), calculate the similarity measure of itself and target area ${\mathrm{\ρ}}_{\mathrm{ij}}$ $(i=i_0-\frac{l}{2}+\frac{n}{2},...,i_0+\frac{l}{2}-\frac{n}{2};j=j_0-\frac{k}{2}+\frac{m}{2},...{,j}_0+\frac{k}{2}-\frac{m}{2}),$ (i ₀, j ₀) be field of search center pixel; Work as ρ _ijobtain maximal value ρ _maxtime, the center pixel of this search window is considered to same place, according to above-mentioned principle, can obtain point set whole pixel matching result point set on registering images $P\{(x_{J^{\′}}^{\′\′i},y_{J^{\′}}^{\′\′i}),i=\mathrm{1,2},...,n\}.$

${\mathrm{\ρ}}_{\max }=\max \left\{{\mathrm{\ρ}}_{\mathrm{ij}}|\begin{array}{c}i=i_0-\frac{l}{2}+\frac{n}{2},...,i_0+\frac{l}{2}-\frac{n}{2}\\ j=j_0-\frac{k}{2}+\frac{m}{2},...,j_0+\frac{k}{2}-\frac{m}{2}\end{array}\right\}---\left(11\right)$

Wherein, similarity measure ρ _ijthe correlation coefficient ρ of the pel array of the m * n centered by match point by target area image and the field of search _kcalculate.

${\mathrm{\ρ}}_k=\frac{{\mathrm{\σ}}_{g{g}^{\′}}}{\sqrt{{\mathrm{\σ}}_{\mathrm{gg}}{\mathrm{\σ}}_{g^{\′}{g}^{\′}}}}(k=\mathrm{0,1},...,m-n)---\left(12\right)$

If the coordinate of impact point in left image is (x, y), in the field of search, the coordinate of certain point is (x ', y '), and (x ', y ') is shown (k with respect to the offset table of (x, y) ₁, k ₂), g (x, y) is the target area image centered by (x, y), g ' (x ', y ') is the image-region of m * n pixel in the field of search centered by (x ', y ') in the field of search, g _ijthe gradation of image for target area coordinate (i, j), the mean value of the target area image greyscale centered by (x, y), the mean value of the target area image gray scale centered by (x ', y '), σ _{gg '}the variance of target area image centered by (x, y) and the image centered by (x ', y '), σ _ggthe variance of the target area image centered by (x, y), σ _{g ' g '}it is the variance of the image centered by (x ', y ') in the field of search.Relational expression between them:

$\begin{array}{c}\stackrel{\_}{g}=\frac{1}{\mathrm{mn}}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{g}_{\mathrm{ij}},\\ {\stackrel{\_}{g}}^{\′}=\frac{1}{\mathrm{mn}}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{g}_{i+k_1,j+k_2,}^{\′}\\ {\mathrm{\σ}}_{\mathrm{gg}}=\frac{1}{\mathrm{mn}}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{g}_{\mathrm{ij}}^2\stackrel{\_}{-g^2}\\ {\mathrm{\σ}}_{g^{\′}{g}^{\′}}=\frac{1}{\mathrm{mn}}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{g_{i+k_1,j+k_2}^{\′}}^{\text{2}}\text{-}{\stackrel{\_}{g}}^{\′}{\stackrel{\_}{g}}^{\′},\\ {\mathrm{\σ}}_{g{g}^{\′}}=\frac{1}{\mathrm{mn}}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}\underset{j=2}{\overset{n}{\mathrm{\Σ}}}{g}_{\mathrm{ij}}{g}_{i+k_1,j+k_2\stackrel{\_}{g}{\stackrel{\_}{g}}^{\′}.}\end{array}---\left(13\right)$

Step 3: least square coupling

Utilize said method can complete point set between left and right two width images to point set pixel matching, in order further to improve matching precision, to point set in each match point, adopt least square coupling, the information in imaging window is carried out to overall adjustment calculating, finally obtain point set the point set of sub-pix ratings match on registering images $P\{(x_J^{\′\′i},y_J^{\′\′i}),i=\mathrm{1,2},...,n\}.$

The least square coupling of left and right image, the step of images match precision being brought up to sub-pixel grade is as follows:

1) geometric distortion is corrected: according to geometry deformation parameter a ₀, a ₁, a ₂, b ₀, b ₁, b ₂the photo coordinate transform of benchmark image window is to registering images array:

$\begin{array}{c}{x}_{J^{\′}}^{\′\′}=a_0+a_1{x}_T^{\′\′}+a_2{y}_T^{\′\′},\\ y_{J^{\′}}^{\′\′}=b_0+b_1{x}_T^{\′\′}+b_2{y}_T^{\′\′}.\end{array}---\left(14\right)$

Consider that reference image can obtain with respect to the linear tonal distortion of registration picture:

g ₁(x″ _J′，y″ _J′)+n ₁(x″ _J′，y″ _J′)＝h ₀+h ₁g ₂(a ₀+a ₁x″ _T+a ₂y″ _T，b ₀+b ₁x″ _T+b ₂y″T)+n ₂(x″ _T，y″ _T)(15)

2) resample: according to formula (15), adopt bilinear interpolation to carry out gray resample and calculate g ₂(x " _t, y " _t), after linearization, can obtain the error equation of least square images match:

V=c ₁dh ₀+ c ₂dh ₁+ c ₃da ₀+ c ₄da ₁+ c ₃da ₂+ c ₆db ₀+ c ₇db ₁+ c ₈db ₂-Δ g (16) wherein, dh ₀, dh ₁, da ₀... da ₂be the corrected value of distortion parameter, Δ g is the gray scale difference of respective pixel;

3) radiometric distortion corrects: utilize the radiometric distortion parameter h being tried to achieve by least square images match error equation ₀, h ₁, above-mentioned resampling result is made to radiative corrections, i.e. h ₀+ h ₁g ₂(x " _t, y " _t);

4) calculate the correlation coefficient ρ between left image-region and the gray scale array of the right image-region after geometric distortion correction and the other distortion correction of spoke.If correlation coefficient ρ is less than the related coefficient of trying to achieve after a front iteration, calculate optimal match point, iteration finishes; Otherwise carry out step 5);

5) adopt least square images match, separate the corrected value dh of changes persuing shape parameter ₀, dh ₁, da ₀...;

6) calculate deformation parameter: establish $h_0^{i-1},h_1^{i-1},a_0^{i-1},a_1^{i-1},...$ A front deformation parameter, $d{h}_0^i,{\mathrm{dh}}_1^i,d{a}_0^i,{\mathrm{da}}_1^i,...$ The corrected value that this iteration is tried to achieve, geometric deformation parameter by following relation, correct:

$\left[\begin{array}{c}1\\ x_2\\ y_2\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ a_0^i& a_1^i& a_2^i\\ b_0^i& b_1^i& b_2^i\end{array}\right]\left[\begin{array}{c}1\\ x\\ y\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ d{a}_0^i& 1+d{a}_1^i& d{a}_2^i\\ d{b}_0^i& {\mathrm{db}}_1^i& 1+d{b}_2^i\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ a_0^{i-1}& a_1^{i-1}& a_2^{i-1}\\ b_0^{i-1}& b_1^{i-1}& b_2^{i-1}\end{array}\right]\left[\begin{array}{c}1\\ x\\ y\end{array}\right],$

$\⇒\begin{array}{c}{a}_0^i=a_0^{i-1}+d{a}_0^i+a_0^{i-1}d{a}_1^i+b_0^{i-1}d{a}_2^i\\ a_1^i=a_1^{i-1}+a_1^{i-1}d{a}_1^i+b_1^{i-1}d{a}_2^i\\ a_2^i=a_2^{i-1}+a_2^{i-1}d{a}_1^i+b_2^{i-1}d{a}_2^i\\ b_0^i=b_0^{i-1}+d{b}_0^i+a_0^{i-1}d{b}_1^i+b_0^{i-1}d{b}_2^i\\ b_1^i=b_1^{i-1}+a_1^{i-1}d{b}_1^i+b_1^{i-1}d{b}_2^i\\ b_2^i\text{=}{b}_2^{i-1}+a_2^{i-1}d{b}_1^i+b_2^{i-1}d{b}_2^i\end{array}---\left(17\right)$

Radiometric distortion parameter corrects by following relation:

$\begin{array}{c}\left[\begin{array}{c}1\\ g_1\end{array}\right]=\left[\begin{array}{cc}1& 0\\ {\mathrm{dh}}_0^i& 1+{\mathrm{dh}}_1^i\end{array}\right]\left[\begin{array}{cc}1& 0\\ h_0^{i-1}& h_1^{i-1}\end{array}\right]\left[\begin{array}{c}1\\ g_2\end{array}\right]\\ \begin{array}{}\end{array}\begin{array}{}\end{array}\⇒\begin{array}{c}\begin{array}{c}{h}_0^i=h_0^{i-1}+{\mathrm{dh}}_0^i+h_0^{i-1}{\mathrm{dh}}_1^i,\\ h_1^i=h_1^{i-1}+h_1^{i-1}{\mathrm{dh}}_1^i.\end{array}\end{array}\end{array}---\left(18\right)$

7) known according to the Precision Theory of least square coupling, coordinate precision depends on the gradient of gradation of image according to gradient square for power, in benchmark image window, coordinate is made to weighted mean:

$\begin{array}{c}{x}_t=\mathrm{\Σ}{x}_T^{\′\′}\·{{\stackrel{\·}{g}}_{x_T^{\′\′}}}^2/\mathrm{\Σ}{{\stackrel{\·}{g}}_{x_T^{\′\′}}}^2,\\ y_t=\mathrm{\Σ}{y}_T^{\′\′}\·{{\stackrel{\·}{g}}_{y_T^{\′\′}}}^2/\mathrm{\Σ}{{\stackrel{\·}{g}}_{y_T^{\′\′}}}^2\end{array}---\left(19\right)$

Using it as coordinate of ground point, and the geometric transformation parameter that identical point coordinates can be tried to achieve by least square images match is tried to achieve:

$\begin{array}{c}{x}_J^{\′\′}=a_0+a_1{x}_t+a_2{y}_t,\\ y_J^{\′\′}{b}_0+b_1{x}_t+b_2{y}_t.\end{array}---\left(20\right)$

By (20) formula, can calculate successively point set the sub-pixel match point of every bit, finally obtains point set $P\{(x_J^{\′\′i},y_J^{\′\′i}),i=\mathrm{1,2},...,n\}.$

S4, sensing point coordinate calculate: the high precision matching characteristic point obtaining according to least squares adjustment, utilizes forward intersection algorithm to calculate sensing point position.

According to the set of assigned points of left core line image mate point set with it utilize forward intersection algorithm to calculate sensing point position; Concrete steps are:

(1) any point in set of assigned points in left core line image as shown in Figure 2, at left camera core line image space coordinate system O _lcoordinate under-XYZ coordinate system be (x " _lP, y " _lP, z " _lP), the position of sensing point namely.Due in set of assigned points in left core line image a bit by mating to right core line image from left core line image, establish it the concentrated coordinate of point is x " _l, y " _l(being T=L), in the point that mates with it be x " _r, y " _r(being J=R).Left camera intrinsic parameter f _l, x _l0, y _l0with right camera intrinsic parameter f _r, x _r0, y _r0, and right camera is with respect to the outer parameter (ω of left camera core line image space coordinate system _rL, K _rL, φ _rL, X _rL, Y _rL, Z _rL), equal Accurate Calibrations, is known quantity in advance, two cameras can obtain following collinearity condition equation to unknown point P:

$\left\{\begin{array}{c}{x}_L^{\″}-x_{L0}=-f_L\frac{x_{\mathrm{LP}}^{\″}}{z_{\mathrm{LP}}^{\″}}\\ y_L^{\″}-y_{L0}=-f_L\frac{y_{\mathrm{LP}}^{\″}}{z_{\mathrm{LP}}^{\″}}\\ x_R^{\″}-x_{R0}=-f_R\frac{r_{11}(x_{\mathrm{LP}}-X_{\mathrm{RL}})+r_{12}(y_{\mathrm{LP}}^{\″}-Y_{\mathrm{RL}})+r_{13}(z_{\mathrm{LP}}^{\″}-Z_{\mathrm{RL}})}{r_{31}(x_{\mathrm{LP}}^{\″}-X_{\mathrm{RL}})+r_{32}(y_{\mathrm{LP}}^{\″}-Y_{\mathrm{RL}})+r_{33}(y_{\mathrm{LP}}^{\″}-Z_{\mathrm{RL}})}\\ y_R^{\″}-y_{R0}=-f_r\frac{R_{21}(x_{\mathrm{LP}}^{\″}-X_{\mathrm{RL}})+r_{22}(y_{\mathrm{LP}}^{\″}-Y_{\mathrm{RL}})+r_{23}(y_{\mathrm{LP}}^{\″}-Z_{R}L)}{r_{31}(x_{\mathrm{LP}}^{\″}-X_{\mathrm{RL}})+r_{32}(y_{\mathrm{LP}}^{\″}-Y_{\mathrm{RL}})+r_{33}(y_{\mathrm{LP}}^{\″}-Z_{\mathrm{RL}})}\end{array}\right.---\left(22\right)$

Wherein:

$R{\left(r_{\mathrm{ij}}\right)}_{3\×3}=\left[\begin{array}{ccc}\cos {\mathrm{\κ}}_{\mathrm{RL}}\cos {\mathrm{\φ}}_{\mathrm{RL}}-\sin {\mathrm{\ω}}_{\mathrm{RL}}\sin {\mathrm{\κ}}_{\mathrm{RL}}\sin {\mathrm{\φ}}_{\mathrm{RL}}& -\sin {\mathrm{\κ}}_{\mathrm{RL}}\cos {\mathrm{\φ}}_{\mathrm{RL}}-\sin {\mathrm{\φ}}_{\mathrm{RL}}\sin {\mathrm{\ω}}_{\mathrm{RL}}\cos {\mathrm{\κ}}_{\mathrm{RL}}& -\sin {\mathrm{\φ}}_{\mathrm{RL}}\cos {\mathrm{\ω}}_{\mathrm{RL}}\\ \cos {\mathrm{\ω}}_{\mathrm{RL}}\sin {\mathrm{\κ}}_{\mathrm{RL}}& \cos {\mathrm{\ω}}_{\mathrm{RL}}\cos {\mathrm{\κ}}_{\mathrm{RL}}& -\sin {\mathrm{\ω}}_{\mathrm{RL}}\\ \sin {\mathrm{\φ}}_{\mathrm{RL}}\cos {\mathrm{\κ}}_{\mathrm{RL}}+\sin {\mathrm{\ω}}_{\mathrm{RL}}\sin {\mathrm{\κ}}_{\mathrm{RL}}\cos {\mathrm{\φ}}_{\mathrm{RL}}& -\sin {\mathrm{\φ}}_{\mathrm{RL}}\sin {\mathrm{\κ}}_{\mathrm{RL}}+\sin {\mathrm{\ω}}_{\mathrm{RL}}\cos {\mathrm{\κ}}_{\mathrm{RL}}\cos {\mathrm{\φ}}_{\mathrm{RL}}& \cos {\mathrm{\ω}}_{\mathrm{RL}}\cos {\mathrm{\φ}}_{\mathrm{RL}}\end{array}\right]$

In (22) formula, the internal and external parameter of two video cameras is known, thus only have unknown point coordinate (x " _lP, y " _lP, z " _lP) be unknown number, two video cameras have been set up 4 equations and have been solved 3 unknown numbers, thus can to (22) formula set up error equation carry out least square resolve to obtain unknown point three-dimensional coordinate (x " _lP, y " _lP, z " _lP).

(2) (22) formula is carried out to linearization process, the error equation that obtains left core line image and right core line image point coordinate is shown in formula 23:

$\begin{array}{c}{v}_{x_L^{\′\′}}=-\frac{\∂x_L^{\′\′}}{\∂x_{\mathrm{LP}}^{\′\′}}d{x}_{\mathrm{LP}}^{\′\′}-\frac{\∂x_L^{\′\′}}{\∂y_{\mathrm{LP}}^{\′\′}}d{y}_{\mathrm{LP}}^{\′\′}-\frac{\∂x_L^{\′\′}}{\∂z_{\mathrm{LP}}^{\′\′}}d{z}_{\mathrm{LP}}^{\′\′}-l_{x_L^{\′\′}}\\ v_{y_L^{\′\′}}=-\frac{\∂y_L^{\′\′}}{\∂x_{\mathrm{LP}}^{\′\′}}d{x}_{\mathrm{LP}}^{\′\′}-\frac{\∂y_L^{\′\′}}{\∂y_{\mathrm{LP}}^{\′\′}}d{y}_{\mathrm{LP}}^{\′\′}-\frac{\∂y_L^{\′\′}}{\∂z_{\mathrm{LP}}^{\′\′}}d{z}_{\mathrm{LP}}^{\′\′}-l_{y_L^{\′\′}}\\ v_{x_R^{\′\′}}=-\frac{\∂x_R^{\′\′}}{\∂x_{\mathrm{RP}}^{\′\′}}d{x}_{\mathrm{LP}}^{\′\′}-\frac{\∂x_R^{\′\′}}{\∂y_{\mathrm{RP}}^{\′\′}}d{y}_{\mathrm{LP}}^{\′\′}-\frac{\∂x_R^{\′\′}}{\∂z_{\mathrm{RP}}^{\′\′}}d{z}_{\mathrm{LP}}^{\′\′}-l_{x_R^{\′\′}}\\ v_{y_R^{\′\′}=-\frac{\∂y_R^{\′\′}}{\∂x_{\mathrm{RP}}^{\′\′}}d{x}_{\mathrm{LP}}^{\′\′}-\frac{\∂y_R^{\′\′}}{\∂y_{\mathrm{RP}}^{\′\′}}d{y}_{\mathrm{LP}}^{\′\′}-\frac{\∂y_R^{\′\′}}{\∂z_{\mathrm{RP}}^{\′\′}}d{z}_{\mathrm{LP}}^{\′\′}-l_{y_R^{\′\′}}}\end{array}---\left(23\right)$

be respectively x " _l, y " _l, x " _r, y " _rcorresponding residual error, be respectively x " _l, y " _l, x " _r, y " _rapproximate value.

To each picture point, can list two error equations, and this point appears in 2 width sequential images, lists 2*2 equation, available matrix representation, see formula 24:

V=AX-L (24) wherein, $A=\left[\begin{array}{ccc}-\frac{\∂x_L^{\′\′}}{\∂x_{\mathrm{LP}}^{\′\′}}& -\frac{\∂x_L^{\′\′}}{\∂y_{\mathrm{LP}}^{\′\′}}& \frac{\∂x_L^{\′\′}}{\∂z_{\mathrm{LP}}^{\′\′}}\\ -\frac{\∂y_L^{\′\′}}{\∂x_{\mathrm{LP}}^{\′\′}}& -\frac{\∂y_L^{\′\′}}{\∂y_{\mathrm{LP}}^{\′\′}}& -\frac{\∂y_L^{\′\′}}{\∂z_{\mathrm{LP}}^{\′\′}}\\ -\frac{\∂x_R^{\′\′}}{\∂x_{\mathrm{RP}}^{\′\′}}& -\frac{\∂x_R^{\′\′}}{\∂y_{\mathrm{RP}}^{\′\′}}& -\frac{\∂x_R^{\′\′}}{\∂z_{\mathrm{RP}}^{\′\′}}\\ -\frac{\∂y_R^{\′\′}}{\∂x_{\mathrm{RP}}^{\′\′}}& -\frac{\∂y_R^{\′\′}}{\∂y_{\mathrm{RP}}^{\′\′}}& -\frac{\∂y_R^{\′\′}}{\∂z_{\mathrm{RP}}^{\′\′}}\end{array}\right],X={[x_{\mathrm{LP}}^{\′\′},y_{\mathrm{LP}}^{\′\′},z_{\mathrm{LP}}^{\′\′}]}^T,V={\text{[}{v}_{x_L^{\′\′}}\text{,}{v}_{y_L^{\′\′}}\text{,}{v}_{x_R^{\′\′}}\text{,}{v}_{y_R^{\′\′}}\text{]}}^T,$ it is the constant term of error equation.

(2) solve error equation (being formula 24), its solution is shown in formula 25:

X＝(A ^TA) ^-1A ^TL (25)

Can calculate thus (x " _lP, y " _lP, z " _lP), this point coordinate is the sensing point position of driving.

The above; it is only the embodiment in the present invention; but protection scope of the present invention is not limited to this; any people who is familiar with this technology is in the disclosed technical scope of the present invention; can understand conversion or the replacement expected; all should be encompassed in of the present invention comprise scope within, therefore, protection scope of the present invention should be as the criterion with the protection domain of claims.

Claims (3)

1. the planet car sensing point localization method based on fisheye camera, is characterized in that, comprises the following steps:

S1, left and right fisheye camera original image is converted to respectively to the central projection image under central projection pattern;

S2, central projection image is carried out to core line correct to process, be converted to left and right core line image;

S3, from left core line image extract minutiae, on right core line image, according to described unique point, utilize point that correlation coefficient process therefrom searches out related coefficient maximum as match point, then utilize least square method, the match point that correlation coefficient process is extracted carries out compensating computation, makes the matching precision of left and right core line image reach sub-pixel grade.

S4, the high precision matching characteristic point obtaining according to least squares adjustment, utilize forward intersection algorithm to calculate sensing point position.

2. a kind of planet car sensing point localization method based on fisheye camera as claimed in claim 1, is characterized in that,

S1 is specially: arbitrary spatial point P is converted to central projection pattern hypograph coordinate (x ', y ') at the picture coordinate (x, y) of fisheye camera imaging; If O ' is optical center, O ' X ' Y ' plane is that fish eye lens is as plane, f is the focal length of fisheye camera, O ' Z is primary optical axis, the O ' of take sets up image space coordinate system O '-X ' Y ' Z as coordinate origin, arbitrary spatial point P, under image space coordinate system corresponding to fisheye camera coordinate be (x ', y ', z); O ' P line and the projection sphere p that intersects at a point, the orthogonal projection of p point to O ' X ' Y ' as in plane, obtain picture point p ', coordinate is (x, y), (x, y) be a P under image space coordinate system corresponding to fisheye camera coordinate (x ', y ', spherical projection z), spherical equation is x ^{' 2}+ y ^{' 2}+ z ²=f ²; Supposing has a virtual picture plane OXY at the plane place of Z=f, corresponding change in coordinate axis direction is parallel with the middle coordinate axis of picture plane O ' X ' Y ', O ' P line and its intersection point are p "; p " virtual central projection picture point for a P, this p " under the central projection of point, as coordinates table, be shown (x ', y '); P, p " and 3 collinearity equations of following central projection of O ', between each coordinate, there is following relation:

$\left\{\begin{array}{c}{x}^{\′}=\frac{f\·x}{\sqrt{f^2-x^2-y^2}}\\ y^{\′}=\frac{f\·y}{\sqrt{f^2-x^2-y^2}}\end{array}\right.---\left(1\right)$

Making the picpointed coordinate of the left image of original fisheye camera is (x _l, y _l), the picpointed coordinate of the right image of original fisheye camera is (x _r, y _r), according to formula (1), have the picpointed coordinate of their correspondences on central projection image be respectively (x ' _l, y ' _l), (x ' _r, y ' _r), that is,

$\left\{\begin{array}{c}{x}_L^{\′}=\frac{f\·x_L}{\sqrt{f^2-x_L^2-y_L^2}}\\ y_L^{\′}=\frac{f\·y_R}{\sqrt{f^2-x_L^2-y_L^2}}\end{array}\right.$

$\left\{\begin{array}{c}{x}_R^{\′}=\frac{f\·x_R}{\sqrt{f^2-x_L^2-y_L^2}}\\ y_R^{\′}=\frac{f\·y_R}{\sqrt{f^2-x_R^2-y_R^2}}\end{array}\right.$

3. a kind of planet car sensing point localization method based on fisheye camera as claimed in claim 1 or 2, is characterized in that, S2 is specially: establish the picpointed coordinate of picture point on left and right core line image be respectively (x " _l, y " _l), (x " _r, y " _r), the picpointed coordinate on they and central projection image (x ' _l, y ' _l), (x ' _r, y ' _r) transformational relation be:

Wherein, N _turnfor the transition matrix between left fisheye camera core line image and its original image, N ' _turnfor the transition matrix between right fisheye camera core line image and its original image, $\left(\begin{array}{c}{X}_0^{\′}\\ Y_0^{\′}\end{array}\right)$ For the translation parameters of changing between left fisheye camera core line image and its original image, $\left(\begin{array}{c}{X}_0^{\′\′}\\ Y_0^{\′\′}\end{array}\right)$ For the translation parameters of changing between right fisheye camera core line image and its original image.