CN101196999A - Target model based on straight line and positioning method - Google Patents

Abstract

The invention discloses a target model for positioning a three-dimensional object and a positioning method, which can obtain accurate depth and attitude information of the three-dimensional object by using a rectangular pyramid model for positioning. Four side faces and a placing bottom face of the rectangular pyramid target model are in alternate black and white colors, twelve straight lines are arranged on the rectangular pyramid target model, and the rectangular pyramid target model is placed by rotating the square bottom face for 45 degrees around the axis. And combining the model to design a positioning algorithm. And (3) establishing a positioning mathematical model according to angle information between straight lines and length information of the straight lines for any three straight lines intersecting two points on the rectangular pyramid target model (wherein the distances between the two intersecting points and the optical center of the camera are unequal). And solving the model based on the geometric meaning and by combining a step length acceleration method. The invention has the advantages of easy image processing, strong anti-shielding capability, free line selection, high positioning precision, simple positioning algorithm and strong real-time property.

Description

A kind of object module and localization method based on straight line

Technical field

The present invention relates to image processing techniques, specifically a kind of object module and localization method based on straight line.

Background technology

The visible sensation method that can obtain posture information has many, and binocular or many orders cube vision is typically arranged, based on monocular vision of model etc.Monocular vision based on model is meant that only utilizing a video camera to take single photo positions work.Because of it only needs a vision sensor, so this method is simple in structure, also avoided the visual field in the stereoscopic vision little simultaneously, problems such as three-dimensional coupling difficulty.Its precondition is that the geometric model of object is known.

The current aspect of model based on model monocular vision location is divided into several classes such as point, straight line and curve.Comparatively speaking, more to monocular vision localization method research based on a feature, and based on present study also fewer of the monocular vision localization method of linear feature.And in some particular environment, the linear feature localization method has certain advantage than a characteristic positioning method.The advantage of linear feature shows following several respects: the image of (1) physical environment comprises a lot of linear features.(2) linear feature is higher than a Feature Extraction precision on image.(3) the anti-ability of blocking of linear feature is more intense.Simultaneously with respect to more senior geometric properties (curve, ellipse etc.), linear feature also has advantage, is in particular in following several respects: (1) in the image of physical environment, straight line is more common than other senior geometric properties, also easier extraction of while around.(2) mathematic(al) representation of straight line is simpler, and it is higher to deal with efficient.Therefore in general, adopt linear feature to carry out the advantage that vision localization has further feature in some aspects and do not had, having a wide range of applications aspect high precision, the real-time autonomous positioning realizing.

The linear feature Position Research concentrates on three aspects: (1) is based on the cooperative target design of straight line; (2) straight line location algorithm; (3) iterative method.

(1) cooperative target based on straight line designs

At present, the design of cooperative target is based on design a little more, based on the cooperative target design studies of straight line seldom.Straight line has superiority at the image processing method mask, and therefore the cooperative target design based on straight line has certain application value in actual applications.

(2) straight line location algorithm

At present, studying maximum in theory is the problem of utilizing three-way location, i.e. perspective-three-line is hereinafter to be referred as (P3L) problem.For the P3L problem, most of scholar is by the vertical mathematical model of setting up with the object straight line of normal vector on the explanation plane of image straight line and video camera photocentre formation.Have two not parallel and discord photocentre coplanes in three straight lines of the definite object pose of this method requirement at least, and then set up three nonlinear equations that constitute by three straight lines, its mathematical model can be described below: suppose that the rotation matrix between camera coordinate system and the object coordinates system is R, to space line L _i, known its direction vector under object coordinates system is , under camera coordinate system, have through rotational transform $S_i=R{\stackrel{\→}{n}}_i.$ Obtain about the relational expression of rotation matrix R by mathematical model be: ${\stackrel{\→}{N}}_i\·R{\stackrel{\→}{n}}_i=0,$ Therefore as long as pass through the projection equation of three straight lines, just can obtain three parameters of R matrix by the group of solving an equation, can be in the hope of the R matrix.This method has solved effectively uses linear feature how to carry out the problem of vision localization, and weak point wherein is the Nonlinear System of Equations complexity, and positioning error is bigger.

(3) method for solving

At present, the method for finding the solution mainly contains two kinds, and a kind of is analytic solution, and a kind of is numerical solution.For analytic solution (closed solutions) method, Dhome (document 1:M.Dhome, M.Richetic, J.-T.Lapreste, and G.Rives, " Determination of the attitude of 3-D objects from a singleperspective view; " IEEE Trans.Pattern Anal.Machine Intell, vol.11, no.12, pp.1266-1278,1989) and Chen (document 2:H.Chen, " Pose determination from lineto plane correspondences:existence solution and closed form solutions; " IEEE Trans.Pattern Anal.Machine Intell, vol 13, no.6pp.530-541.) derive one eight order polynomial by any three lines in space by setting up special model coordinate systems, and this eight order polynomial can be determined the pose of object by the method for closed solutions.Radu Horaud (document 3:Horaud.R.Conio.B.Leboullcux O and Lacolle, B, " An analytic solution for theperspective 4-point Problem; " Computer Vision Graphics and imageprocessing, vol 47, no.1, pp.33-44,1989) obtain a quartic polynomial for non-coplanar three straight lines, also can determine the pose of object at last by the mode of closed solutions by the method for iteration.The weakness of analytic solution method is the problem of separating more, and positioning error is big.Many scholars put forward the problem of separating that various alternative manner solves analytic solution more, just the numerical solution method.For the numerical solution method, Yuan (document 4:J S-C Yuan. " A general photogrammetric method fordetermining object position and orientation; " IEEE Transactions on Roboticsand Automation, vol.5, no.2, pp.129～142) centralized calculation rotation matrix R parameter is separated rotation matrix R parameter in suggestion from the T parameter.Rotation matrix R represents by orthogonal matrix, and separating is that common root by six quadratic polynomials represents that this common root obtains by the Newton iteration gradient method, yet the author notices when using the Newton iteration gradient method and locally optimal solution can occur.Lowe (document 5:D Lowe. " Three-dimensional Object Recognition from SingleTwo-dimensional Images; " Artificial Intelligence, vol 31, pp.355～395,1987.) use Newton iteration method to estimate direction and the location parameter of object with respect to video camera, the same with the method for Yuan (4), Lowe notices some problems of Newton iteration method, and he has studied how to handle initial value and stability problem in the article afterwards.Liu (document 6:Y Liu, T S Huang, O DFaugeras. " Determination of camera location from 2-D to 3-D line and pointcorrespondences; " IEEE Trans.Pattern Anal.Machine Intell, vol 12, no 1, pp.28～37,1990) use alternately process of iteration to find the solution vision parameter, rotation matrix is represented with Eulerian angle, and the author is the linearization of error power function, and they notice that this method works together effective when three Eulerian angle are littler than 30 degree.Thai Quynh Phong (document 7:Thai quynhphong, " Object Pose from 2-D to 3-D Point and Line Correspondences; " International Joural of Computer Vision, vol 15, pp.225-243,1995) use the optimal region optimization method to find the solution.Stephane (document 9:Stephane Christy and Radu Horaud, " Iterative Pose Computation from Line Correspondences; " Computer Visionand Image Understanding, vol 73, no.1, pp.137-144,1999) use the initial value that approaches true value to carry out iteration, use iterative algorithm that the pose under weak perspective projection and the parallel perspective projection is estimated, and geometric moment rank of matrix in the pilot process through discussion, the layout of straight line on the target has been proposed some constraints.These methods can be applied in the situation of three above straight line location.Yet local minimum appears in numerical solution method easily in optimizing process, can not guarantee uniqueness of solution; And calculated amount is big, and iteration time is long, is not suitable for being applied in the real-time system.

Summary of the invention

In order to overcome above-mentioned deficiency, the purpose of this invention is to provide a kind of object module based on straight line, that this model image is handled is convenient, route selection is free more, anti-blocks that ability is strong, bearing accuracy is high.And designed a kind of localization method at the straight line on the model.This localization method computational accuracy height, real-time.

To achieve these goals, technical scheme of the present invention is as follows:

The present invention is based on the object module of straight line: be the rectangular pyramid model, set one and place the bottom surface; Described placement bottom surface is that four split squares piece together a whole square structure, and described four split squares are chequered with black and white setting on color, and four foursquare intersections of split are four straight lines that are positioned at bottom center and intersect on the bottom surface; Described rectangular pyramid structure by four bases and four sides totally eight straight lines constitute, four sides of rectangular pyramid model are chequered with black and white setting on color; Described rectangular pyramid model is placed on the bottom surface around whole square bottom surface axle center rotation 45 degree, and the whole square of the side of rectangular pyramid and bottom surface also is chequered with black and white setting on color;

Wherein: on described rectangular pyramid model, 12 straight lines that are used for extracting choose the cooperative targets that three straight lines that meet at 2 constitute 32 kinds of wiring combinations; The distance that meets at 2 two intersection points of three straight lines and video camera photocentre is unequal.

Use the localization method of described object module based on straight line: is the image capture object with 12 on the rectangular pyramid model at the chequered with black and white straight line that is provided with under the background, from 32 kinds of wiring combinations of 12 straight lines, choose one group of three straight line that meet at 2 as location feature, length information according to angle information between the straight line and straight line, in image, extract the image straight-line equation of three straight lines that meet at 2, find the solution the location mathematical model based on geometric meaning and in conjunction with the step-length accelerated process, obtain three-dimensional cooperative target position and attitude; Wherein: the distance that meets at 2 two intersection points of three straight lines and video camera photocentre is unequal;

Described location mathematical model comprises: suppose: space line L _iThe unit direction vector of (i=1,2,3) is V _i(A _i, B _i, C _i), two intersection points of three straight lines are the first intersection point p ₂, the second intersection point p ₃, the first intersection point p wherein ₂On image, be projected as q ₂(x ₂, y ₂, z ₂), the second intersection point p ₃On image, be projected as q ₃(x ₃, y ₃, z ₃), the first intersection point p ₂, the second intersection point p ₃Coordinate be respectively (k ₂x ₂, k ₂y ₂, k ₂z ₂), (k ₃x ₃, k ₃y ₃, k ₃z ₃), k wherein ₂, k ₃Be undetermined coefficient; Space line L _iBe projected as image straight line l at the plane of delineation _i, its straight-line equation is: a _ix _i+ b _iy _i+ c _i=0, image straight line l _iOn be t arbitrarily a bit _i(x _i, y _i, f), image straight line l _iDirection vector be v _i(b _i, a _i, 0); The geometrical constraint of known spatial model is: the first intersection point p ₂With the distance of video camera photocentre greater than the second intersection point p ₃Distance with the video camera photocentre; The first intersection point p ₂With the second intersection point p ₃Distance be a, formula is: | p ₂p ₃|=a; Article one straight line L in three straight lines in space ₁With second straight line L ₂Between angle be α, second straight line L ₂With the 3rd straight line L ₃Angle is β, article one straight line L ₁With the 3rd straight line L ₃Between angle be γ; By article one straight line L ₁With the 3rd straight line L ₃Between angle be the constraint condition of γ, obtain variable k ₂Expression formula f (k ₂)=0: variable k wherein ₂Be the first intersection point p ₂The distance and the first intersection point p with video camera photocentre o ₂On image, be projected as q ₂(x ₂, y ₂, z ₂) with the ratio of the distance of video camera photocentre o;

Described variable k ₂, i.e. the first intersection point p ₂The distance and the first intersection point p with video camera photocentre o ₂On image, be projected as q ₂(x ₂, y ₂, z ₂) with the finding the solution of the ratio of the distance of video camera photocentre o in based on the alternative manner of geometric meaning be: at article one straight line L ₁The explanation planar S at place ₁With second straight line L ₂The explanation planar S at place ₂Between intersection J ₃Starting point p of last setting _2i, at second straight line L ₂The explanation planar S at place ₂With the 3rd straight line L ₃The explanation planar S at place ₃Between intersection J ₄On find and starting point p _2iDistance is the terminating point p of length a _3iStarting point p _2i, terminating point p _3iConnect and compose second iteration straight line L _2iThrough starting point p _2iExplaining planar S ₁On find one and second iteration straight line L _2iArticle one iteration straight line L of angled α _1iThrough terminating point p _3iPoint is being explained planar S ₃On find one and second iteration straight line L _2iThe 3rd the iteration straight line L of angled β _3iAlong with starting point p _2iAnd the continuous increase of distance between the video camera photocentre o, article one iteration straight line L _1iWith the 3rd iteration straight line L _3iBetween angle constantly increase, when the angle between them satisfies given value γ, starting point p _2iIterate to the tram, whole iterative process finishes.

The principle of the invention:

The present invention adopts four sides of rectangular pyramid object module and places chequered with black and white color on the coated on bottom side, can increase the contrast of image, is easier to extract straight line.Simultaneously, have 32 kinds of wiring combinations on the rectangular pyramid cooperative target, can choose any one group of linear feature combination according to the difference of location occasion and position, the route selection of cooperative target is free more.Cooperative target has 32 kinds of wiring combinations strengthens the anti-ability of blocking of cooperative target.Have 32 kinds of wiring combinations on the cooperative target, increased the redundancy of information, the redundancy of information can improve bearing accuracy.The putting position of rectangular pyramid cooperative target is around rectangular pyramid axle center rotation 45 degree with square bottom surface.Through a large amount of experimental verifications, this putting position has high orientation precision than other putting positions.

Compared with prior art, the present invention has more following advantage:

1. Flame Image Process is easier.Four sides of rectangular pyramid are chequered with black and white, increased picture contrast, and straight line extracts more convenient.

2. the anti-ability of blocking is strong.Linear feature itself has the stronger anti-ability of blocking than a feature.Simultaneously, the anti-ability of blocking of 32 kinds of linear feature combination also having increased object modules.

3. route selection is free more.32 kinds of linear feature combinations are arranged on the rectangular pyramid model, can choose different straight line integrated positionings, make route selection free more according to different occasion needs.

4. precision height.32 kinds of linear feature combinations are arranged on the rectangular pyramid model, and the redundancy of information can improve bearing accuracy.

5. the location algorithm mathematical formulae is simple, real-time.The present invention uses the straight line with special geometric position relation to set up the figure place model of halting, and the specific position relation between the straight line can be simplified derivation algorithm.Mathematical model is simpler, and adopts the step-length accelerated process to search for based on geometric meaning, and iterations is few, has saved computing time, so improved the real-time of system.

Description of drawings

Fig. 1-the 1st, the side view of the rectangular pyramid modelling of one embodiment of the invention.

Fig. 1-2 is the vertical view of rectangular pyramid modelling.

Fig. 2 is the pictorial diagram (as: rectangular pyramid) of rectangular pyramid model of the present invention.

Fig. 3 positioning principle synoptic diagram of the present invention (as: three straight line location).

Fig. 4 is a method for solving synoptic diagram of the present invention.

Embodiment

Below in conjunction with accompanying drawing and example the present invention is described in further detail.

Shown in Fig. 1-1,1-2, the placement bottom surface of rectangular pyramid model: be that four squares piece together a whole square structure, these four split squares are chequered with black and white setting on color, four foursquare intersections of split are formed on four straight lines that bottom center intersects on the bottom surface, and the chequered with black and white color of square makes the Flame Image Process of four intersections easier.The rectangular pyramid model rotates 45 degree with square bottom surface and is placed on the bottom surface around the axle center.

Rectangular pyramid model: by four bases and four side totally eight rectilinear(-al)s.Four sides of rectangular pyramid model are chequered with black and white colors, make the Flame Image Process of four sides simpler.The square of the side of rectangular pyramid and bottom surface also constitutes chequered with black and white color simultaneously, makes the Flame Image Process on four bases of rectangular pyramid model more convenient.Therefore, on the rectangular pyramid model, have 12 straight lines of being convenient to extract.From 12 straight lines, choose one group of three straight line (wherein: the distance of two intersection points and video camera photocentre is unequal) that meet at 2, the position of the target of cooperating and Attitude Calculation.

In these 12 straight lines, the combination that meets at three straight lines (wherein: the distance of two intersection points and video camera photocentre is unequal) of 2 has 32 kinds.Can be according to the needs of different occasions, any one straight line combination of choosing wherein positions.Use a video camera to absorb the image of this cooperative target, in image, extract the image straight-line equation of three straight lines that meet at 2,, just can realize the position and the Attitude Calculation of cooperative target again by the location algorithm of following narration.

Introduce below by any one group and meet at the method that three straight lines (wherein: the distance of two intersection points and video camera photocentre is unequal) of 2 position, see Fig. 3, hypothesis space straight line L _iThe unit direction vector of (i=1,2,3) is V _i(A _i, B _i, C _i).If three two of straight line intersection points are the first intersection point p ₂, the second intersection point p ₃, the first intersection point p wherein ₂On image, be projected as q ₂(x ₂, y ₂, z ₂), the second intersection point p ₃On image, be projected as q ₃(x ₃, y ₃, z ₃).The first intersection point p ₂, the second intersection point p ₃Coordinate be respectively (k ₂x ₂, k ₂y ₂, k ₂z ₂), (k ₃x ₃, k ₃y ₃, k ₃z ₃), k wherein ₂, k ₃Be undetermined coefficient.If space line L _iBe projected as image straight line l at the plane of delineation _iIts straight-line equation is: a _ix _i+ b _iy _i+ c _i=0, image straight line l so _iOn be t arbitrarily a bit _i(x _i, y _i, f), image straight line l _iDirection vector be v _i(b _i, a _i, 0).The geometrical constraint of known spatial model is: the first intersection point p ₂With the distance of video camera photocentre greater than the second intersection point p ₃Distance with the video camera photocentre.The first intersection point p ₂With the second intersection point p ₃Distance be a, formula is: | p ₂p ₃|=a.Article one straight line L in three straight lines in space ₁With second straight line L ₂Between angle be α, second straight line L ₂With the 3rd straight line L ₃Angle is β, article one straight line L ₁With the 3rd straight line L ₃Between angle be γ.See Fig. 4.

Perpendicular to the normal vector N that explains the plane _i=(N _I1, N _I2, N _I3), can utilize 1 t on the image straight line _iDirection vector ot with the line of video camera photocentre o _iDirection vector v with the image straight line _iMultiplication cross obtain, known:

Ot _i=(x _iy _iF), v _i=(b _ia _i0), N is arranged then _i=v _i* ot _i=(a _iF b _iF c _i)

For article one straight line L ₁: its vectorial mould is 1, obtains:

$A_1^2+B_1^2+C_1^2=1---\left(1\right)$

Article one, straight line L ₁Be positioned at by image straight line l ₁On the explanation plane that is constituted with photocentre, obtain:

A ₁N ₁₁+B ₁N ₁₂+C ₁N ₁₃＝0 (2)

By article one straight line L ₁With second straight line L ₂Between angle be α constraint condition, obtain:

A ₁A ₂+B ₁B ₂+C ₁C ₂＝cos(α) (3)

Article one, straight line L ₁Direction vector A ₁, B ₁, C ₁With second straight line L ₂Direction vector A ₂, B ₂, C ₂Express:

$A_1=\frac{-(\mathrm{mn}+\mathrm{pq})\±\sqrt{{(\mathrm{mn}+\mathrm{pq})}^2-(n^2+q^2+1)(p^2+m^2-1)}}{(n^2+q^2+1)}$

B ₁＝m+nA ₁ (4)

C ₁＝p+qA ₁

Wherein: $m=\frac{\cos \left(\mathrm{\α}\right)N_{13}}{B_2{N}_{13}-N_{12}{C}_2},$ $n=\frac{N_{11}{C}_2-A_2{N}_{13}}{B_2{N}_{13}-N_{12}{C}_2},$ $p=\frac{-\cos \left(\mathrm{\α}\right)N_{12}}{B_2{N}_{13}-C_2{N}_{12}},$ $q=\frac{A_2{N}_{12}-B_2{N}_{11}}{B_2{N}_{13}-C_2{N}_{12}}$

For second straight line L ₂: second straight line L ₂Direction vector by the first intersection point p ₂, the second intersection point p ₃Be expressed as:

$L_2=\left(\begin{array}{c}{A}_2\\ B_2\\ C_2\end{array}\right)=\left(\begin{array}{c}{k}_3{x}_3-k_2{x}_2\\ k_3{y}_3-k_2{y}_2\\ k_3{z}_3-k_2{z}_2\end{array}\right)---\left(1\right)$

By constraint condition: the first intersection point p ₂With the second intersection point p ₃Distance be a, formula | p ₂p ₃|=a obtains:

(k ₃z ₃-k ₂z ₂) ²+(k ₃y ₃-k ₂y ₂) ²+(k ₃x ₃-k ₂x ₂) ²＝a ² (6)

Arrangement (6) formula gets:

$k_3=\frac{-f_1{k}_2\±\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(f_3{k_2}^2-a^2)}}{2f_1}---\left(7\right)$

$f_1=x_2^2+y_2^2+z_2^2$

Wherein: f ₂=-2 (x ₁x ₂+ y ₁y ₂+ z ₁z ₂)

$f_3=x_1^2+y_1^2+z_1^2$

By constraint condition (the first intersection point p ₂With the distance of video camera photocentre greater than the second intersection point p ₃Distance with the video camera photocentre), so equation (7) is got negative sign, obtain:

$k_3=\frac{-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(f_3{k_2}^2-a^2)}}{2f_1}---\left(8\right)$

Equation (8) substitution equation (5), equation (5) can be write as about a variable k ₂Expression formula, be made as equation (9):

$L_2=\left(\begin{array}{c}{A}_2\\ B_2\\ C_2\end{array}\right)=\left(\begin{array}{c}{k}_3{x}_3-k_2{x}_2\\ k_3{y}_3-k_2{y}_2\\ k_3{z}_3-k_2{z}_2\end{array}\right)=\left(\begin{array}{c}\frac{-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(f_3{k_2}^2-a^2)}}{2f_1}{x}_3-k_2{x}_2\\ \frac{-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(f_3{k_2}^2-a^2)}}{2f_1}{y}_3-k_2{y}_2\\ \frac{-f_1{k}_2-\sqrt{{\left(g_2{k}_2\right)}^2-4f_1(f_3{k_2}^2-a^2)}}{2f_1}{z}_3-k_2{z}_2\end{array}\right)---\left(9\right)$

For the 3rd straight line L ₃: its vectorial mould is 1, obtains:

$A_3^2+B_3^2+C_3^2=1---\left(10\right)$

Article three, straight line L ₃ Be positioned at by image straight line l ₃On the explanation plane that is constituted with photocentre, obtain:

A ₃N ₃₁+B ₃N ₃₂+C ₃N ₃₃＝0 (11)

And the 3rd straight line L ₃With second straight line L ₂Between angle be β constraint condition, obtain:

A ₃A ₂+B ₃B ₂+C ₃C ₂＝cos(β) (12)

Article three, straight line L ₃Direction vector A ₃, B ₃, C ₃With second straight line L ₂Direction vector A ₂, B ₂, C ₂Express:

$A_3=\frac{-(\mathrm{gh}+\mathrm{wl})\±\sqrt{{(\mathrm{gh}+\mathrm{wl})}^2-(h^2+l^2+1)(w^2+g^2-1)}}{(h^2+l^2+1)}$

B ₃＝g+hA ₃ (13)

C ₃＝w+lA ₃

Wherein: $g=\frac{\cos \left(\mathrm{\β}\right)N_{33}}{B_2{N}_{33}-N_{32}{C}_2},$ $h=\frac{N_{31}{C}_2-A_2{N}_{33}}{B_2{N}_{33}-N_{32}{C}_2},$ $w=\frac{-\cos \left(\mathrm{\β}\right)N_{32}}{B_2{N}_{33}-C_2{N}_{32}},$ $l=\frac{A_2{N}_{32}-B_2{N}_{31}}{B_2{N}_{33}-C_2{N}_{32}}$

Equation (9) substitution equation (4), then equation (4) can be write as about a variable k ₂Expression formula, be made as equation (14):

$A_1=\frac{-(\mathrm{mn}+\mathrm{pq})\±\sqrt{{(\mathrm{mn}+\mathrm{pq})}^2-(n^2+q^2+1)(p^2+m^2-1)}}{(n^2+q^2+1)}$

B ₁＝m+nA ₁ (14)

C ₁＝p+qA ₁

Wherein:

$m=\frac{2f_1\cos \left(\mathrm{\α}\right)N_{13}}{((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(k_3{k_2}^2-a^2)})y_3-2f_1{k}_2{y}_2)N_{13}-N_{12}((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(f_3{k_2}^2-a^2)})z_3-2f_1{k}_2{z}_2)}$

$n=\frac{2f_1{N}_{11}{C}_2-((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(f_3{k_2}^2-a^2)})x_3-k_2{x}_2)N_{13}}{((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(k_3{k_2}^2-a^2)})y_3-2f_1{k}_2{y}_2)N_{13}-N_{12}((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(f_3{k_2}^2-a^2)})z_3-2f_1{k}_2{z}_2)}$

$p=\frac{2f_1\cos \left(\mathrm{\α}\right)N_{12}}{((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(k_3{k_2}^2-a^2)})y_3-2f_1{k}_2{y}_2)N_{13}-N_{12}((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(f_3{k_2}^2-a^2)})z_3-2f_1{k}_2{z}_2)}$

$q=\frac{((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(f_3{k_2}^2-a^2)})x_3-2f_1{k}_2{x}_2)N_{12}-((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(f_3{k_2}^2-a^2)})y_3-2f_1{k}_2{y}_2)N_{11}}{((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(k_3{k_2}^2-a^2)})y_3-2f_1{k}_2{y}_2)N_{13}-N_{12}((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(f_3{k_2}^2-a^2)})z_3-2f_1{k}_2{z}_2)}$

Equation (9) substitution equation (13), then equation (13) can be write as about a variable k ₂Expression formula, be made as equation (15):

$A_3=\frac{-(\mathrm{gh}+\mathrm{wl})\±\sqrt{{(\mathrm{gh}+\mathrm{wl})}^2-(h^2+l^2+1)(w^2+g^2-1)}}{(h^2+l^2+1)}$

B ₃＝g+hA ₃ (15)

C ₃＝w+lA ₃

Wherein:

$g=\frac{2f_1\cos \left(\mathrm{\β}\right)N_{33}}{((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(k_3{k_2}^2-a^2)})y_3-2f_1{k}_2{y}_2)N_{33}-N_{32}((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(f_3{k_2}^2-a^2)})z_3-2f_1{k}_2{z}_2)}$

$h=\frac{2f_1{N}_{31}{C}_2-((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(f_3{k_2}^2-a^2)})x_3-k_2{x}_2)N_{33}}{((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(k_3{k_2}^2-a^2)})y_3-2f_1{k}_2{y}_2)N_{33}-N_{32}((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(f_3{k_2}^2-a^2)})z_3-2f_1{k}_2{z}_2)}$

$w=\frac{-2f_1\cos \left(\mathrm{\β}\right)N_{32}}{((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(k_3{k_2}^2-a^2)})y_3-2f_1{k}_2{y}_2)N_{33}-N_{32}((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(f_3{k_2}^2-a^2)})z_3-2f_1{k}_2{z}_2)}$

$l=\frac{((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(f_3{k_2}^2-a^2)})x_3-2f_1{k}_2{x}_2)N_{32}-((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(f_3{k_2}^2-a^2)})y_3-2f_1{k}_2{y}_2)N_{31}}{((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(k_3{k_2}^2-a^2)})y_3-2f_1{k}_2{y}_2)N_{33}-N_{32}((-f_1{k}_2-\sqrt{{\left(f_2{k}_2\right)}^2-4f_1(f_3{k_2}^2-a^2)})z_3-2f_1{k}_2{z}_2)}$

By article one straight line L ₁With the 3rd straight line L ₃Between angle be the constraint condition of γ, obtain:

A ₁A ₃+B ₁B ₃+C ₁C ₃＝cos(γ) (16)

Equation (14) (15) substitution equation (16), obtain about a variable k ₂Expression formula f (k ₂)=0.

(2) alternative manner

At variable k ₂Find the solution, designed a kind of alternative manner, i.e. the first intersection point p based on geometric meaning and step-length accelerated process ₂The distance and the first intersection point p with video camera photocentre o ₂On image, be projected as q ₂(x ₂, y ₂, z ₂) the finding the solution of ratio of distance in as follows based on the alternative manner of geometric meaning:

See Fig. 4, at article one straight line L ₁The explanation planar S at place ₁With second straight line L ₂The explanation planar S at place ₂Between intersection J ₃Starting point p of last setting _2i, at second straight line L ₂The explanation planar S at place ₂With the 3rd straight line L ₃The explanation planar S at place ₃Between intersection J ₄On find and starting point p _2iDistance is the terminating point p of length a _3iStarting point p _2i, terminating point p _3iConnect and compose second iteration straight line L _2iThrough starting point p _2iExplaining planar S ₁On find one and second iteration straight line L _2iArticle one iteration straight line L of angled α _1iThrough terminating point p _3iExplaining planar S ₃On find one and second iteration straight line L _2iThe 3rd the iteration straight line L of angled β _3iAlong with starting point p _2iAnd the continuous increase of distance between the video camera photocentre o, article one iteration straight line L _1iWith the 3rd iteration straight line L _3iBetween angle constantly increase, when the angle between them satisfies given value γ, starting point p _2iIterate to the tram, whole iterative process finishes.

Example: the intrinsic parameter matrix of known video camera is $\left(\begin{array}{ccc}787.887& 0& 0\\ 0& 787.887& 0\\ 0& 0& 1\end{array}\right),$ The field angle of video camera is 36 * 36 degree.One group of three straight line (wherein: the distance of two intersection points and video camera photocentre is unequal) that meet at 2 is l at the image straight line of the plane of delineation on the rectangular pyramid model _i(i=1,2,3).Article three, image straight line l _iDirection vector be respectively v ₁(0.1970,0.1970,0), v ₂(0.1970,0,0), v ₃(0,0.1970,0).

Can calculate the correct position of rectangular pyramid cooperative target and attitude is T (0,0,850), R (0,0,0) by the location algorithm of above narration.Therefore, meet at the calculating that three straight lines (wherein: the distance of two intersection points and video camera photocentre is unequal) of 2 can be realized cooperative target position and attitude by any one group.

In a word, the present invention has designed a kind of rectangular pyramid object module, and chequered with black and white side and the design of placement bottom surface can increase picture contrast, and it is easier that straight line is extracted; 32 groups of linear feature combinations are arranged on the rectangular pyramid object module, increased the anti-ability of blocking of object module, route selection is free more; Simultaneously also can increase quantity of information, improve bearing accuracy.Specific position relation by particular arrangement straight line group is set up the location mathematical model, and expression formula is simple, is easier to find the solution; Employing is found the solution based on geometric meaning and in conjunction with the step-length accelerated process, has avoided nonlinear optimal problem dexterously, has avoided occurring local minimum, finds the solution stablely, and has improved counting yield, makes system have very high real-time.

Claims (7)

1. the object module based on straight line is characterized in that: be the rectangular pyramid model, set one and place the bottom surface; Described placement bottom surface is that four split squares piece together a whole square structure, and described four split squares are chequered with black and white setting on color, and four foursquare intersections of split are four straight lines that are positioned at bottom center and intersect on the bottom surface; Described rectangular pyramid structure by four bases and four sides totally eight straight lines constitute, four sides of rectangular pyramid model are chequered with black and white setting on color; Described rectangular pyramid model is placed on the bottom surface around whole square bottom surface axle center rotation 45 degree, and the whole square of the side of rectangular pyramid and bottom surface also is chequered with black and white setting on color.

2. according to the described object module of claim 1, it is characterized in that based on straight line: on described rectangular pyramid model, 12 straight lines that are used for extracting choose the cooperative targets that three straight lines that meet at 2 constitute 32 kinds of wiring combinations.

3. according to the described object module based on straight line of claim 1, it is characterized in that: wherein: the distance that meets at 2 two intersection points of three straight lines and video camera photocentre is unequal.

4. an application rights requires the localization method of 1 described object module based on straight line, it is characterized in that: is the image capture object with 12 on the rectangular pyramid model at the chequered with black and white straight line that is provided with under the background, from 32 kinds of wiring combinations of 12 straight lines, choose one group of three straight line that meet at 2 as location feature, length information according to angle information between the straight line and straight line, in image, extract the image straight-line equation of three straight lines that meet at 2, find the solution the location mathematical model based on geometric meaning and in conjunction with the step-length accelerated process, obtain three-dimensional cooperative target position and attitude.

5. according to the localization method of the described object module based on straight line of claim 4, it is characterized in that: the distance that wherein meets at 2 two intersection points of three straight lines and video camera photocentre is unequal.

6. according to the localization method of the described object module based on straight line of claim 2, it is characterized in that: wherein locate mathematical model and comprise: suppose: space line L _iThe unit direction vector of (i=1,2,3) is V _i(A _i, B _i, C _i), two intersection points of three straight lines are the first intersection point p ₂, the second intersection point p ₃, the first intersection point p wherein ₂On image, be projected as q ₂(x ₂, y ₂, z ₂), the second intersection point p ₃On image, be projected as q ₃(x ₃, y ₃, z ₃), the first intersection point p ₂, the second intersection point p ₃Coordinate be respectively (k ₂x ₂, k ₂y ₂, k ₂z ₂), (k ₃x ₃, k ₃y ₃, k ₃z ₃), k wherein ₂, k ₃Be undetermined coefficient; Space line L _iBe projected as image straight line l at the plane of delineation _i, its straight-line equation is: a _ix _i+ b _iy _i+ c _i=0, image straight line l _iOn be t arbitrarily a bit _i(x _i, y _i, f), image straight line l _iDirection vector be v _i(b _i, a _i, 0); The geometrical constraint of known spatial model is: the first intersection point p ₂With the distance of video camera photocentre greater than the second intersection point p ₃Distance with the video camera photocentre; The first intersection point p ₂With the second intersection point p ₃Distance be a, formula is: | p ₂p ₃|=a; Article one straight line L in three straight lines in space ₁With second straight line L ₂Between angle be α, second straight line L ₂With the 3rd straight line L ₃Angle is β, article one straight line L ₁With the 3rd straight line L ₃Between angle be γ; By article one straight line L ₁With the 3rd straight line L ₃Between angle be the constraint condition of γ, obtain variable k _xExpression formula f (k ₂)=0: variable k wherein ₂Be the first intersection point p ₂The distance and the first intersection point p with video camera photocentre o ₂On image, be projected as q ₂(x ₂, y ₂, z ₂) with the ratio of the distance of video camera photocentre o.

7. according to the localization method of the described object module based on straight line of claim 4, it is characterized in that: variable k wherein ₂, i.e. the first intersection point p ₂The distance and the first intersection point p with video camera photocentre o ₂On image, be projected as q ₂(x ₂, y ₂, z ₂) with the finding the solution of the ratio of the distance of video camera photocentre o in based on the alternative manner of geometric meaning be: at article one straight line L ₁The explanation planar S at place ₁With second straight line L ₂The explanation planar S at place ₂Between intersection J ₃Starting point p of last setting _2i, at second straight line L ₂The explanation planar S at place ₂With the 3rd straight line L ₃The explanation planar S at place ₃Between intersection J ₄On find and starting point p _2iDistance is the terminating point p of length a _3iStarting point p _2i, terminating point p _3iConnect and compose second iteration straight line L _2iThrough starting point p _2iExplaining planar S ₁On find one and second iteration straight line L _2iArticle one iteration straight line L of angled α _1iThrough terminating point p _3iPoint is being explained planar S ₃On find one and second iteration straight line L _2iThe 3rd the iteration straight line L of angled β _3iAlong with starting point p _2iAnd the continuous increase of distance between the video camera photocentre o, article one iteration straight line L _1iWith the 3rd iteration straight line L _3iBetween angle constantly increase, when the angle between them satisfies given value γ, starting point p _2iIterate to the tram, whole iterative process finishes.

Images