CN105809687A - Monocular vision ranging method based on edge point information in image - Google Patents

Abstract

The invention relates to a monocular vision ranging method based on edge point information in an image and belongs to the unmanned aerial vehicle navigation and positioning technical field. The method includes the following steps that: two frames of images are selected from an image sequence captured by a downward-looking monocular camera fixedly connected with an unmanned aerial vehicle so as be adopted to construct an initial map and an initial depth graph, a first frame is adopted as a first key frame in the map, and a camera coordinate system corresponding to the first frame is adopted as a world coordinate system, and initialization is completed; and three threads, namely, a motion estimation thread, a map construction thread and a depth graph estimation thread are carried out parallelly, wherein the motion estimation thread aligns known map and depth graph information with a current frame so as to obtain a ranging result, and optimizes the existing map information according to the ranging result, and the map construction thread and the depth graph estimation thread operate simultaneously so as to maintain the map and depth graph information. According to the method of the invention, the multi-core architecture of a modern processor is fully utilized, and the edge point information in the image can be effectively utilized, and based on corner point information, the efficiency of the algorithm is improved. The method has higher adaptability.

Description

A kind of based on the monocular vision ranging method of edge dot information in image

Technical field

The invention belongs to Navigation of Pilotless Aircraft field of locating technology, particularly to a kind of monocular vision ranging method that make use of edge dot information in image.

Background technology

In many application scenarios, people can not rely on GPS to be accurately positioned, as between indoor, city building or jungle, area, mountain valley or even outer celestial body.Utilizing vision to be navigated is meet very much intuition: in nature, all animals having eyes all utilize vision to carry out some form of navigation, the insecticide such as fly, Apis judges the relative motion between self and target by light stream, and in the most of the time, people judges the position being in by the scenery seen.

Under the premise not using GPS, for the consideration of cost and weight, small unmanned plane majority adopts Inertial Measurement Unit and the navigation scheme of single camera combination, it is also possible to be equipped with ultrasonic sensor and baroceptor to obtain absolute altitude information.The unmanned plane of this configuration can realize entirely autonomous way point and follow the tracks of.

Carrying the carrier of single or multiple photographic head, input merely with its image, the process that displacement is estimated is called vision ranging VO (VisualOdometry).Application includes robot, augmented reality and automatic Pilot etc..There is similarity in this process and traditional wheel odometry.Wheel ranging calculates mileage by the rotation of accumulative wheel, the vision ranging change by perception input picture, incrementally estimates carrier pose.In effective service requirement environment of vision ranging algorithm, illumination is sufficient, and scene texture is enough abundant.

Monocular vision ranging is merely with single photographic head as input, and system configuration is simple, and the ability adapting to environmental scale change is better than multi-vision visual system.Existing monocular vision ranging method, Corner Feature in general image carries out mating between frame with frame, (Corner Feature is the key character of image, pixel prominent in some attribute in representative image cannot to adapt to lack the scene of Corner Feature.In scene in FIG, Corner Feature occurs in the point of intersection of straight line, rare numbers and existence repeatability in structure more, and the existing monocular vision ranging algorithm based on Corner Feature will likely inefficacy).Additionally, these Corner Features only account for the sub-fraction of the whole pixel of image, from the utilization rate of image information, the performance of monocular vision ranging can also improve.

The term that the present invention relates to illustrates as follows:

Frame: in vision ranging field, custom claims the piece image obtained to be a frame, and such as, the image that camera previous moment obtains is called former frame, and the image that camera current time obtains is called present frame, and the continuous two width images that camera obtains are called consecutive frame etc.；

Key frame: owing to the frame per second of Current camera is higher, pose change between consecutive frame is often smaller, in order to strengthen the accuracy of estimation, generally take the strategy of key frame, namely, in certain pose excursion, newly obtained image only carries out aliging to estimate current pose with a certain specific frame, and only after beyond certain scope, we just take new specific frame to carry out the image alignment of next stage, and namely claiming these particular frames being used for carrying out image alignment is key frame；

Reference frame: the frame of present image of being used for aliging is called the reference frame of present image；

Map: in vision ranging field, saves known environmental information (such as the position of calculated point, the image etc. that obtains), is called map.Map can mate as successive image, the prior information of estimation to be to increase the precision of ranging.

Summary of the invention

The present invention is directed to the deficiency of prior art, propose a kind of based on the monocular vision ranging method of edge dot information in image, the edge dot information in image can be effectively utilized, in conjunction with angle point information (as auxiliary), the scene lacking Corner Feature is had higher adaptability.

It is a kind of based on the method for the unmanned plane monocular vision ranging of edge dot information in image that the present invention proposes, it is characterised in that the method includes initializing and the estimation of parallel processing, map structuring and depth map estimation；Specifically include following steps:

1) initializing: select two frames to build initial map and ID figure from the image sequence caught depending on monocular camera being connected with unmanned plane, map is with one group of key frame set { r_mAnd one group of three-dimensional feature point set { p_iRepresent, wherein r represents that key frame, subscript m represent m-th key frame, m is positive integer, and p represents three-dimensional feature point, and subscript i represents i-th three-dimensional feature point, i is positive integer, and the some set of the two-dimensional angular of its correspondence, corresponding to the Corner Feature in image, is designated as by three-dimensional feature pointWherein u_cRepresenting angle point, subscript i represents i-th angle point, and i is positive integer；Depth map is made up of the uncertainty of edge point coordinates, edge point depth value and depth value, depth map and key frame one_to_one corresponding, and depth map is usedRepresenting, wherein D represents the depth map that edge point is corresponding, and subscript m represents m width depth map, for positive integer, u_eRepresent edge point, d_eRepresent the depth value of edge point, σ_eRepresenting the uncertainty that the edge point degree of depth is reciprocal, subscript i represents i-th edge point, for positive integer；

Wherein, the first frame is taken as first key frame in map, and camera coordinates system corresponding for the first frame is taken as world coordinate system, complete to initialize；

2) after initializing map and depth map information, carry out estimation, map structuring and depth map parallel and estimate three threads: wherein, estimation thread utilizes known map and depth map information, carry out aliging with present frame and obtain ranging result, and according to ranging result, existing cartographic information is optimized, map structuring and depth map estimate that thread runs to safeguard map and depth map information simultaneously.

The feature of the present invention and beneficial effect:

The present invention propose based on the monocular vision ranging method of edge dot information in image, using the image caught depending on monocular camera under being connected with unmanned plane as input, through airborne computer computing, the result of output is the ranging result of unmanned plane；By estimation thread, map structuring thread and depth map, the method estimates that three parallel threads of thread realize；Wherein estimation thread utilizes the existing information in map and depth map to do the pose of the estimation current kinetic that aligns with present frame, and it is the core of ranging algorithm, it is necessary to assure hard real-time；Map structuring thread is for safeguarding the three-dimensional feature map corresponding to key frame information and image Corner Feature；Depth map estimates that thread is used for safeguarding the estimation of Depth of edge dot information in key frame.

Estimation, map structuring and depth map are estimated to be separately operable in three threads by the present invention, make full use of the multicore architecture of modern processors, improve efficiency of algorithm.Not having the process of feature extraction and matching in motion estimation algorithm, adopt quick feature point extraction algorithm during map structuring, the pixel that in depth map estimation, extracting directly brightness step is bigger, this all effectively reduces amount of calculation.Simultaneously in the initial estimation process of motion estimation algorithm, make use of the edge dot information in image, and devise the depth map transmission method of edge point accordingly.Edge point in image is to enrich a lot of characteristics of image than angle point, and algorithm adopts edge dot information can reduce the algorithm dependence to Corner Feature in initial estimation, strengthens the adaptive capacity to environment of algorithm.

Accompanying drawing explanation

Fig. 1 is the scene that the monocular vision ranging method based on Corner Feature is likely to lose efficacy；

Fig. 2 is the overview flow chart improving monocular vision ranging method based on edge point that the present invention proposes；

Fig. 3 is the estimation thread flow figure of the embodiment of the present invention；

Fig. 4 is the map structuring thread flow figure of the embodiment of the present invention；

The depth map that Fig. 5 is the embodiment of the present invention estimates thread flow figure；

Detailed description of the invention

Below in conjunction with drawings and Examples, the present invention is described in detail.

The present invention propose a kind of based on the overall procedure of the method for the unmanned plane monocular vision ranging of edge dot information in image as shown in Figure 2, it is characterised in that, including initializing and the estimation of parallel processing, map structuring and depth map are estimated；Specifically include following steps:

1) initializing: select two frames to build initial map and ID figure from the image sequence caught depending on monocular camera being connected with unmanned plane, in the present embodiment, map is with one group of key frame set { r_mAnd one group of three-dimensional feature point set { p_iRepresent, wherein r represents that key frame, subscript m represent m-th key frame, for positive integer, p represents three-dimensional feature point, and subscript i represents i-th three-dimensional feature point, for positive integer, the two-dimensional angular point set of its correspondence, corresponding to the Corner Feature in image, is generally designated as by three-dimensional feature pointWherein u_cRepresenting angle point, subscript i represents i-th angle point, for positive integer；

In the present embodiment, each width key frame in map calculating the depth map of its correspondence, depth map is made up of the uncertainty of edge point coordinates, edge point depth value and depth value, and namely depth map is usedRepresenting, wherein D represents the depth map that edge point is corresponding, and subscript m represents m width depth map, for positive integer (depth map and key frame one_to_one corresponding), u_eRepresent edge point, d_eRepresent the depth value of edge point, σ_eRepresenting the uncertainty that the edge point degree of depth is reciprocal, subscript i represents i-th edge point, for positive integer.

Wherein, first frame is taken as first key frame in map, and camera coordinates system corresponding for the first frame is taken as world coordinate system, and (world coordinate system refers to that the coordinate system such as east northeast ground coordinate system of relative real world has the coordinate system determining transformation relation, it is world coordinate system that the present embodiment elects camera coordinates system corresponding to the first frame as convenient calculating), complete to initialize；

2) after initializing map and depth map information, carry out estimation, map structuring and depth map parallel and estimate three threads: wherein, estimation thread utilizes known map and depth map information, carry out aliging with present frame and obtain ranging result, and according to ranging result, existing cartographic information is optimized, map structuring and depth map estimate that thread runs to safeguard map and depth map information simultaneously；Three threads are described in detail below:

21) estimation thread: the current frame image newly obtained for camera, utilizes the edge point depth information in current key frame, carries out the image alignment based on motion model, obtain present frame pose T in world coordinate system_{K, w}Initial estimationWherein T represents pose, and subscript k represents that present frame is the kth frame image that camera obtains, and for positive integer, subscript w represents world coordinate system, subscript-expression initial estimation, utilizes initial estimation, it is possible to by three-dimensional feature point { p existing in map_iBe incident upon in present image, and obtained and { p by Block-matching_iCorresponding two-dimentional angle pointWithFor measuring value,For initial value, successively to present frame pose T in world coordinate system w_{K, w}It is optimized with three-dimensional feature point position relevant in map.This estimation specific embodiment is as it is shown on figure 3, include:

S11: note current frame image is I_k, wherein I represents the image that camera obtains, and first current for unmanned plane pose is initialized as the pose of previous moment, namelyWith current key frame r_mAs reference frame, minimize I with Gauss-Newton iterative algorithm_kAnd r_mBetween weighted intensity residual error such as formula (1-1):

Obtain present frame I_kRelative to reference frame r_mPose T_{K, m}, whereinRepresent reference frame r_mMiddle degree of depth inverse uncertainty is less than the uncertainty threshold value (the 1/200 of degree of depth inverse maximum estimated value) set and at present frame visible edge point set, u_iRepresenting the pixel in image, wherein u represents that pixel, subscript i represent ith pixel point, for positive integer, w_i(u_i) represent pixel u_iWeights, be taken asσ_iRepresent the uncertainty of the degree of depth of ith pixel point, pixel u_iGray scale residual error δ R (T_{K, m}, u_i) for formula (1-2):

Wherein π is projection function, the three-dimensional point in camera coordinates system is projected in two dimensional image coordinate system, π^-1For inverse projection function,WithRepresent that pixel * is at frame I respectively_kWith frame r_mIn gray value, in conjunction with reference frame r_mPose T relative to world coordinate system_{M, w}, then the initial estimation of present frame pose can be calculatedFor formula (1-3):

$T_{k,w}^{-}=T_{k,m}\·T_{m,w}---(1-3)$

If participating in the edge point quantity of weighting less than the edge point threshold value (the present embodiment value as 300) set, going to step S12, being otherwise directly entered step S13；

S12: be estimated as initial value with the pose obtained in step S11, by former frame I_k-1Reference frame as present frame carries out image alignment, minimizes I by Gauss-Newton iteration optimization algorithms_kAnd I_k-1Between gray scale residual error obtain the present frame pose T relative to former frame_{K, k-1}, such as formula (1-4):

WhereinRepresent the degree of depth in frame k-1 it is known that and the visible angle point set of present frame (projected by the three-dimensional feature point in map and obtain).In conjunction with the former frame pose T relative to world coordinate system_{K-1, w}, then the initial estimation of present frame pose can be calculatedFor formula (1-5):

$T_{k,w}^{-}=T_{k,k-1}\·T_{k-1,w}---(1-5)$

Now, by present frame I_kElect the new key frame reference frame as follow-up one section of image as；The input information of thread is estimated simultaneously as map structuring and depth map；

S13:(utilizes the initial estimation of present frame poseBy the three-dimensional feature spot projection in map to present frame, one group of two-dimensional coordinate can be obtained, but due toThere is error, these two-dimensional coordinates are not necessarily the three-dimensional feature point actual position in present frame imaging.) according to initial estimationJudge the three-dimensional feature point set { p that can be observed by present frame in map_i, obtain it at reference frame r_mThe angular coordinate of middle correspondence is estimatedWithFor initial value, calculate { p with image tracking algorithm KLT (Kanade-Lucas-TomasiFeatureTracker, the public algorithm for conventional)_iIn the true imaging position of present frame

$(u_{c_i},\mathrm{\δ}\stackrel{\‾}{R})=\arg {\min }_{(u_{c_i},\mathrm{\δ}\stackrel{\‾}{R})}\frac{1}{2}\left|\right|R_{I_k}\left(u_{c_i}\right)-A_i\·R_{I_m}\left(u_{c_i}^{\′}\right)+\mathrm{\δ}\stackrel{\‾}{R}|{|}^2,\∀i---(1-6)$

Wherein, A_iRepresent the transformation matrix that reference segment is transformed to present image.Represent that the average gray with reference to the coupling segment in segment and present image is poor, be used for eliminating the illumination effect (three-dimensional feature point { p that this step establishes in map_iAnd image in two dimension angle pointThe corresponding relation of sub-pixel precision)；

S14: estimate through the initial pose of the step S13 two-dimensional coordinate obtained with present frameNo longer meeting geometric Projection Constraint, i.e. formula (1-7):

$\left|\right|{\mathrm{\δu}}_{c_i}\left|\right|=\left|\right|u_{c_i}-\mathrm{\π}(T_{k,w}^{-}\·p_i)\left|\right|\≠0---(1-7)$

WithFor initial value, by minimizing re-projection error to T_{K, w}It is updated the present frame pose T obtained_{K, w}Namely it is the final result of estimation, such as formula (1-8):

$T_{k,w}=\arg {\min }_{T_{k,w}}\frac{1}{2}{\mathrm{\Σ}}_i\left|\right|u_{c_i}-\mathrm{\π}(T_{k,w}\·p_i)|{|}^2---(1-8)$

T_{K, w}It is the final result that ranging obtains.Go to step S15 afterwards, the present frame pose T simultaneously obtained by step S14_{K, w}Turn map structuring thread respectively and depth map estimates that thread calculates angle point depth value and calculates edge point depth value；

S15: utilize the present frame pose T that step S14 obtains_{K, w}, each the three-dimensional feature point in the map can observe present frame is optimized respectively, and the error sum of squares making the projection in all key frames that can observe it of this three-dimensional feature point and true imaging position is minimum namely such as formula (1-9):

$p_i=\arg {\min }_{p_i}\frac{1}{2}{\mathrm{\Σ}}_j\left|\right|u_{c_i}^j-\mathrm{\π}(T_{j,w}\·p_i)|{|}^2,\∀i---(1-9)$

In above formula, subscript j represents can observe three-dimensional feature point p_iJth key frame；

22) map structuring thread, process in two kinds of situation: if the new image obtained is chosen as new key frame at estimation thread step S12, perform step S21, Corner Feature is extracted from the key frame being newly added, the angle point that algorithm is each new extraction creates a depth filter based on probability (namely the degree of depth of angle point is made up of, and observation new each time is all passed through depth filter and updated depth value and the uncertainty thereof of angle point) depth value and uncertainty；When the new image obtained is not selected as key frame, the pose utilizing the estimation thread step S14 present frame obtained performs step S22～S24, utilize the probability depth filter of the information updating Corner Feature of new images, when the degree of depth uncertainty of a certain angle point is less than the uncertainty threshold value set, the three-dimensional feature of its correspondence point is joined in map, this thread implementation is as shown in Figure 4, concrete steps describe as follows: first determine whether whether new images is key frame, if performing step S21, otherwise perform step S22-S24；

S21: when a two field picture is chosen to be key frame, uses FAST algorithm (FeaturesfromAcceleratedSegmentTest, conventional Fast Corner extraction algorithm) to extract this key frame Corner Feature；First being divided by image uniform with equally spaced grid (such as 30x30 pixel), only extract an angle point, make angle point be evenly distributed in image in each grid, sum is less than grid number；For the angle point that each is newCreate a probability depth filter, and by the inverse of its degree of depth(whereinRepresenting the inverse of the degree of depth of angle point, subscript i represents the i-th angle point in current key frame, for positive integer), and the probability of occurrence ρ that this angle point is effectively observed_iThe joint posterior distribution of (wherein ρ represents effective observation probability of angle point, and subscript i represents the i-th angle point in current key frame, for positive integer) defines such as formula (2-1):

$q({\tilde{d}}_i,{\mathrm{\ρ}}_i|a_n,b_n,{\mathrm{\μ}}_n,{\mathrm{\σ}}_n^2):=Beta\left({\mathrm{\ρ}}_i\right|a_n,b_n)N\left({\tilde{d}}_i\right|{\mathrm{\μ}}_n,{\mathrm{\σ}}_n^2)---(2-1)$

Wherein Beta and N represents Beta distribution and normal distribution respectively, and subscript n represents that current probability depth filter has been carried out n subparameter to be updated, for positive integer, parameter a_nAnd b_nIt is the number of times that in recursion renewal process, effectively observation and invalid observation occur respectively, μ_nWithIt is then average and the variance of degree of depth inverse Gauss distribution；When parameter in formula (2-1) is initialized, by a_nAnd b_nInitial value be set to 10, μ_nIt is initialized as the inverse of the mean depth of place image,It is initialized as maximum.

S22: use the image I that the step S14 camera pose obtained is known_k, the estimation of Depth of Corner Feature is updated；The angle point that note is updatedThe key frame at place is r_m, according to present frame I_kAnd r_mRelative pose, at I_kIn determine straight line, and ensure I_kIn withCorresponding pixelOccur on this straight line；This straight line scans for (this process be commonly referred to polar curve search), obtain through the Block-matching of sub-pixel precisionCoordinate after, calculate described angle point with triangulation (principle of measurement in stereoscopic vision)The degree of depth

S23: the degree of depth that step S22 is obtainedInverted isWherein, subscript i represents it is i-th angle point in current key frame, and for positive integer, subscript n represents that this is that the n-th to current angle point depth filter measures and updates, for positive integer, the inverse to the degree of depthProbability distribution model such as formula (2-2):

$p\left({\tilde{d}}_i^n\right|{\tilde{d}}_i,{\mathrm{\ρ}}_i)={\mathrm{\ρ}}_{i}N\left({\tilde{d}}_i^n\right|{\tilde{d}}_i,{\mathrm{\τ}}_i^2)+(1-{\mathrm{\ρ}}_i)U\left({\tilde{d}}_i^n\right|{\tilde{d}}_i^{\min },{\tilde{d}}_i^{\max })---(2-2)$

The implication of above formula is: key frame r_mMiddle i-th angle point is through image I_kTriangulation, if being effectively measure, then the inverse of measurement resultMeet normal distributionWhereinThe degree of depth inverse caused for one pixel error of the plane of delineation calculates variance, if being invalid measurement, thenMeet and be uniformly distributedWhereinMinima that the degree of depth set for the prior information according to current scene is reciprocal and maximum；Pass throughMeasurement update, the degree of depth is reciprocalPosterior probability distribution such as formula (2-3):

$C\×p\left({\tilde{d}}_i^n\right|{\tilde{d}}_i,{\mathrm{\ρ}}_i)q({\tilde{d}}_i,{\mathrm{\ρ}}_i|a_{n-1},b_{n-1},{\mathrm{\μ}}_{n-1},{\mathrm{\σ}}_{n-1}^2)---(2-3)$

Wherein, C is that guarantee probability is distributed normalized constant.Can be calculated by moment-matching method (statistical method, with the approximate another kind distribution of one distribution)

S24: reject and effectively measure probabilityThe angle point of too low (less than 0.1), by uncertainty σ_nMeet and require (σ_nThe 1/200 of the maximum estimated value reciprocal less than the angle point degree of depth) point add in map；

23) depth map estimates thread, processes in two kinds of situation: if the new image obtained is chosen as new key frame at estimation thread step S12, extracts edge point from the key frame being newly added, performs step S31～S33；When newly inputted image is not selected as key frame, utilize the pose of the estimation thread step S14 present frame obtained, perform step S34～S36, update the probability depth filter of edge point by new image information, and reject the edge point that effective observation probability is too low；Performing step as it is shown in figure 5, be described in detail below: first determine whether whether new images is key frame, if performing step S31-S33, otherwise performing step S34-S36；

S31: in the key frame being newly added, utilizes Sobel operator (a kind of universal method carrying out edge point detection) to calculate the shade of gray G that image is horizontal and vertical_xAnd G_y, shade of gray figure usesApproximate.Selecting 2500 to 4000 pixels that shade of gray is maximum from image, when the significant pixel number of gradient is less than 2500, Grads threshold 450 is reduced to original 0.95 times, when number is more than 4000, increase threshold value is original 1.05 times；

S32: remember that new key frame is r_m, previous key frame is r_m-1, this step is by frame r_m-1Depth map D^m-1Propagate new key frame depth maps D^m；r_m-1In, edge pointThe reciprocal average of the degree of depth beThe variance of degree of depth inverse isWherein,Represent the average that the degree of depth of edge point is reciprocal,Representing the variance that the degree of depth is reciprocal, subscript i represents frame r_m-1In i-th edge point.r_m-1To r_mRelative pose be T_{M, m-1}, edge pointAt frame r_mMiddle corresponding sides are along the position putCan be calculated by projection relation obtains such as formula (3-1):

$u_{e_i}^{\′}=\mathrm{\π}(T_{m,m-1}.{\mathrm{\π}}^{-1}(u_{e_i},\frac{1}{{\widetilde{\mathrm{\μ}}}_{e_i}}\left)\right)---(3-1)$

NoteDegree of depth inverse beThe variance of degree of depth inverse is designated asSo:

${\widetilde{\mathrm{\μ}}}_{e_i}^{\′}\left({\widetilde{\mathrm{\μ}}}_{e_i}\right)={({\widetilde{\mathrm{\μ}}}_{e_i}^{-1}-t_z)}^{-1}---(3-2)$

Wherein, t_zRepresent camera translation on optical axis.Thus can deriveThe reciprocal variance of the degree of depthFor:

${\widetilde{\mathrm{\σ}}}_{e_i}^{\′2}=J_{{\widetilde{\mathrm{\μ}}}_{e_i}}\·{\widetilde{\mathrm{\σ}}}_{e_i}^2\·J_{{\widetilde{\mathrm{\μ}}}_{e_i}}^T+{\mathrm{\σ}}_p^2={\left(\frac{{\widetilde{\mathrm{\μ}}}_{e_i}^{\′}}{{\widetilde{\mathrm{\μ}}}_{e_i}}\right)}^4\·{\widetilde{\mathrm{\σ}}}_{e_i}^2+{\mathrm{\σ}}_p^2---(3-3)$

WhereinRepresent uncertainty in traffic, rule of thumb arrange.Due to projection error,Coordinate be not likely to be integer, willIt is tied to r_mIn nearest pixel

S33: each the edge point for extracting in step S31 creates probability depth filter, if 3x3 pixel coverage internal memory is in prior estimate near some edge pointThen its degree of depthAverageInitialize such as formula (3-4):

${\mathrm{\μ}}_{e_i}=\frac{{\mathrm{\Σ}}_i\frac{11}{{\widetilde{\mathrm{\σ}}}_{e_i}^{\′2}{\widetilde{\mathrm{\μ}}}_{e_i}^{\′}}}{{\mathrm{\Σ}}_i\frac{1}{{\widetilde{\mathrm{\σ}}}_{e_i}^{\′2}}}---(3-4)$

The uncertainty that the parameter degree of depth of probability depth filter is reciprocalThen it is initialized as in prior estimate minimum uncertainty, namelyIf there is no prior estimate, then probability depth filter is initialized as mean depth and maximum uncertainty；TakeThen can construct probability depth filter such as formula (3-5) according to formula (2-1) for each edge point:

$q({\tilde{d}}_{e_i},{\mathrm{\ρ}}_i|a_n,b_n,{\mathrm{\μ}}_n,{\mathrm{\σ}}_n^2):=Beta\left({\mathrm{\ρ}}_i\right|a_n,b_n)N\left({\tilde{d}}_{e_i}\right|{\mathrm{\μ}}_n,{\mathrm{\σ}}_n^2)---(3-5)$

Wherein, the definition of (3-5) formula each parameter is similar with (2-1) formula, here the parameters of the probability depth filter of corresponding sides edge point, a_nAnd b_nInitial value be set to 10, μ_n, σ_nIt is initialized asWith

S34: by new key frame r_mIt is set to the reference frame of successive image, utilizes follow-up image to frame r_mIn depth map D_mCarry out measuring and update；Use image I_kTo D^mDuring renewal, the edge point that note is updated isAccording to I_kAnd r_mRelative pose, at I_kIn determine straight line, it is ensured that I_kIn withCorresponding pixelOccur on this straight line；If some shade of gray direction, edge and described rectilinear direction angle are less than threshold value (the present embodiment sets threshold value as 25 degree), obtain through the Block-matching of sub-pixel precisionCoordinate after, calculate the degree of depth of described edge point with triangulation meterPerform step 35)；If described angle is more than this threshold value, then do not search for, return step S34 and check next edge point.

S35: the degree of depth that step S34 is obtainedInverted isWherein, subscript i represents it is i-th edge point in current key frame, and for positive integer, subscript n represents that this is that the n-th to current edge point point depth filter measures and updates, for positive integer, the inverse to the degree of depthProbability distribution model such as formula (3-6):

$p\left({\tilde{d}}_{e_i}^n\right|{\tilde{d}}_{e_i},{\mathrm{\ρ}}_i)={\mathrm{\ρ}}_{i}N\left({\tilde{d}}_{e_i}^n\right|{\tilde{d}}_{e_i},{\mathrm{\τ}}_i^2)+(1-{\mathrm{\ρ}}_i)U\left({\tilde{d}}_{e_i}^n\right|{\tilde{d}}_{e_i}^{\min },{\tilde{d}}_{e_i}^{\max })---(3-6)$

The implication of above formula is: key frame r_mMiddle i-th edge point point is through image I_kTriangulation, if being effectively measure, then the inverse of measurement resultMeet normal distributionIf being invalid measurement, thenMeet and be uniformly distributedWhereinMinima that the degree of depth set for the prior information according to current scene is reciprocal and maximum；Pass throughMeasurement update, the degree of depth is reciprocalPosterior probability distribution such as formula (3-6):

$C\×p\left({\tilde{d}}_{e_i}^n\right|{\tilde{d}}_{e_i},{\mathrm{\ρ}}_i)q\left({\tilde{d}}_{e_i}{\mathrm{\ρ}}_i\right|a_{n-1},b_{n-1},{\mathrm{\μ}}_{n-1},{\mathrm{\σ}}_{n-1}^2)---(2-3)$

Calculated by moment-matching method

S36: reject effectively measure probability too low (Less than 0.1) edge point.

These are only presently preferred embodiments of the present invention, be only used for explaining the present invention, and be not limitation of the present invention.And protection scope of the present invention is not limited thereto; any those of ordinary skill in the art; when without departing from the spirit of the present invention, principle and scope; these embodiments can also be carried out multiple change, modification, amendment and replacement; therefore all equivalent technical methods all should belong to scope of the invention, and protection scope of the present invention should by claims and equivalent limit thereof.

Claims (4)

1. one kind based on the method for the unmanned plane monocular vision ranging of edge dot information in image, it is characterised in that the method includes initializing and the estimation of parallel processing, map structuring and depth map are estimated；Specifically include following steps:

1) initializing: select two frames to build initial map and ID figure from the image sequence caught depending on monocular camera being connected with unmanned plane, map is with one group of key frame set { r_mAnd one group of three-dimensional feature point set { p_iRepresent, wherein r represents that key frame, subscript m represent m-th key frame, m is positive integer, and p represents three-dimensional feature point, and subscript i represents i-th three-dimensional feature point, i is positive integer, and the some set of the two-dimensional angular of its correspondence, corresponding to the Corner Feature in image, is designated as by three-dimensional feature pointWherein u_cRepresenting angle point, subscript i represents i-th angle point, and i is positive integer；Depth map is made up of the uncertainty of edge point coordinates, edge point depth value and depth value, depth map and key frame one_to_one corresponding, and depth map is usedRepresenting, wherein, D represents the depth map that edge point is corresponding, and subscript m represents m width depth map, for positive integer, u_eRepresent edge point, d_eRepresent the depth value of edge point, σ_eRepresenting the uncertainty that the edge point degree of depth is reciprocal, subscript i represents i-th edge point, for positive integer；

Wherein, the first frame is taken as first key frame in map, and camera coordinates system corresponding for the first frame is taken as world coordinate system, complete to initialize；

2) after initializing map and depth map information, carry out estimation, map structuring and depth map parallel and estimate three threads: wherein, estimation thread utilizes known map and depth map information, carry out aliging with present frame and obtain ranging result, and according to ranging result, existing cartographic information is optimized, map structuring and depth map estimate that thread runs to safeguard map and depth map information simultaneously.

2. as claimed in claim 1 method, it is characterised in that described step 2) carry out the estimation thread that estimation, map structuring and depth map estimate in three threads parallel and specifically comprise the following steps that

The current frame image that camera is newly obtained, utilizes the edge point depth information in current key frame, carries out the image alignment based on motion model, obtain present frame pose T in world coordinate system_{K, w}Initial estimationWherein T represents pose, and subscript k represents that present frame is the kth frame image that camera obtains, and k is positive integer, and subscript w represents world coordinate system, subscript-expression initial estimation, utilizes initial estimation, by three-dimensional feature point { p existing in map_iBe incident upon in present image, and obtained and { p by Block-matching_iCorresponding two-dimentional angle pointWithFor measuring value,For initial value, successively to present frame pose T in world coordinate system w_{K, w}It is optimized with three-dimensional feature point position relevant in map；

S11: note current frame image is I_k, wherein I represents the image that camera obtains, and first current for unmanned plane pose is initialized as the pose of previous moment, namelyWith current key frame r_mAs reference frame, minimize I with Gauss-Newton iterative algorithm_kAnd r_mBetween weighted intensity residual error such as formula (1-1):

Obtain present frame I_kRelative to reference frame r_mPose T_{K, m}, whereinRepresent reference frame r_mMiddle degree of depth uncertainty is less than the uncertainty threshold value set and at present frame visible edge point set, u_iRepresenting the pixel in image, wherein u represents that pixel, subscript i represent ith pixel point, for positive integer, w_i(u_i) represent pixel u_iWeights, be taken asσ_iRepresent the uncertainty of the degree of depth of ith pixel point, pixel u_iGray scale residual error δ R (T_{K, m}, u_i) for formula (1-2):

Wherein π is projection function, the three-dimensional point in camera coordinates system is projected in two dimensional image coordinate system, π^-1For inverse projection function,WithRepresent that pixel * is at frame I respectively_kWith frame r_mIn gray value, in conjunction with reference frame r_mPose T relative to world coordinate system_{M, w}, then the initial estimation of present frame pose can be calculatedFor formula (1-3):

$T_{k,w}^{-}=T_{k,m}\·T_{m,w}---(1-3)$

If participating in the edge point quantity of weighting less than the edge point threshold value set, going to step S12, being otherwise directly entered step S13；

S12: be estimated as initial value with the pose obtained in step S11, by former frame I_k-1Reference frame as present frame carries out image alignment, minimizes I by Gauss-Newton iteration optimization algorithms_kAnd I_k-1Between gray scale residual error obtain the present frame pose T relative to former frame_{K, k-1}, such as formula (1-4):

WhereinRepresent the degree of depth in frame k-1 it is known that and in the visible angle point set of present frame in conjunction with the former frame pose T relative to world coordinate system_{K-1, w}, then the initial estimation of present frame pose is calculatedFor formula (1-5):

$T_{k,w}^{-}=T_{k,k-1}\·T_{k-1,w}---(1-5)$

Now, by present frame I_kElect the new key frame reference frame as follow-up one section of image as；The input information of thread is estimated simultaneously as map structuring and depth map；

S13: according to initial estimationJudge the three-dimensional feature point set { p that can be observed by present frame in map_i, obtain it at reference frame r_mThe angular coordinate of middle correspondence is estimatedWithFor initial value, calculate { p with image tracking algorithm KLT_iIn the true imaging position of present frame

$(u_{c_i},\mathrm{\δ}\stackrel{\‾}{R})=\arg {\min }_{(u_{c_i},\mathrm{\δ}\stackrel{\‾}{R})}\frac{1}{2}\left|\right|R_{I_k}\left(u_{c_i}\right)-A_i\·R_{I_m}\left(u_{c_i}^{\′}\right)+\mathrm{\δ}\stackrel{\‾}{R}|{|}^2,\∀i---(1-6)$

Wherein, A_iRepresent the transformation matrix that reference segment is transformed to present image.Represent that the average gray with reference to the coupling segment in segment and present image is poor, be used for eliminating illumination effect；

S14: estimate through the initial pose of the step S13 two-dimensional coordinate obtained with present frameNo longer meet formula (1-7):

$\left|\right|{\mathrm{\δu}}_{c_i}\left|\right|=\left|\right|u_{c_i}-\mathrm{\π}(T_{k,w}^{-}\·p_i)\left|\right|\≠0---(1-7)$

WithFor initial value, by minimizing re-projection error to T_{K, w}It is updated the present frame pose T obtained_{K, w}Namely it is the final result of estimation, such as formula (1-8):

$T_{k,w}=\arg {\min }_{T_{k,w}}\frac{1}{2}{\mathrm{\Σ}}_i\left|\right|u_{c_i}-\mathrm{\π}(T_{k,w}\·p_i)|{|}^2---(1-8)$

T_{K, w}It is the final result that ranging obtains；Go to step S15 afterwards, the present frame pose T simultaneously obtained by step S14_{K, w}Turn map structuring thread respectively and depth map estimates that thread calculates angle point depth value and calculates edge point depth value；

S15: utilize the present frame pose T that step S14 obtains_{K, w}, each the three-dimensional feature point in the map can observe present frame is optimized respectively, and the error sum of squares making the projection in all key frames that can observe it of this three-dimensional feature point and true imaging position is minimum namely such as formula (1-9):

$p_i=\arg {\min }_{p_i}\frac{1}{2}{\mathrm{\Σ}}_j\left|\right|u_{c_i}^j-\mathrm{\π}(T_{j,w}\·p_i)|{|}^2,\∀i---(1-9)$

In above formula, subscript j represents can observe three-dimensional feature point p_iJth key frame.

3. as claimed in claim 2 method, it is characterised in that described step 2) carry out the map structuring thread that estimation, map structuring and depth map estimate in three threads parallel and specifically comprise the following steps that

Described map structuring thread processes in two kinds of situation: if the new image obtained is chosen as new key frame at estimation thread step S12, performs step S21, otherwise utilizes the pose of the estimation thread step S14 present frame obtained, performs step S22-S24；Specifically comprise the following steps that

S21: when a two field picture is chosen to be key frame, uses FAST algorithm to extract this key frame Corner Feature；First being divided by image uniform with equally spaced grid, only extract an angle point, make angle point be evenly distributed in image in each grid, sum is less than grid number；For the angle point that each is newCreate a probability depth filter, and by the inverse of its degree of depthProbability of occurrence ρ with effectively observation_iJoint posterior distribution define such as formula (2-1):

$q({\tilde{d}}_i,{\mathrm{\ρ}}_i|a_n,b_n,{\mathrm{\μ}}_n,{\mathrm{\σ}}_n^2):=Beta\left({\mathrm{\ρ}}_i\right|a_n,b_n)N\left({\tilde{d}}_i\right|{\mathrm{\μ}}_n,{\mathrm{\σ}}_n^2)---(2-1)$

Wherein: whereinRepresent the inverse of the degree of depth of angle point, ρ represents effective observation probability of angle point, subscript i represents the i-th angle point in current key frame, i is positive integer, Beta and N represents Beta distribution and normal distribution respectively, subscript n represents that current probability depth filter has been carried out n subparameter to be updated, and n is positive integer, parameter a_nAnd b_nIt is the number of times that in recursion renewal process, effectively observation and invalid observation occur respectively, μ_nWithIt is then average and the variance of degree of depth inverse Gauss distribution；When parameter in formula (2-1) is initialized, by a_nAnd b_nInitial value be set to 10, μ_nIt is initialized as the inverse of the mean depth of place image,It is initialized as maximum；

S22: use the image I that the step S14 camera pose obtained is known_k, the estimation of Depth of Corner Feature is updated；The angle point that note is updatedThe key frame at place is r_m, according to present frame I_kAnd r_mRelative pose, at I_kIn determine straight line, and ensure I_kIn withCorresponding pixelOccur on this straight line；This straight line carries out polar curve search, obtains through the Block-matching of sub-pixel precisionCoordinate after, calculate described angle point with triangulation meterThe degree of depth

S23: the degree of depth that step S22 is obtainedInverted isWherein, subscript i represents it is i-th angle point in current key frame, and i is positive integer, and subscript n represents that this is that the n-th to current angle point depth filter measures and updates, and n is positive integer, the inverse to the degree of depthProbability distribution model such as formula (2-2):

$p\left({\tilde{d}}_i^n\right|{\tilde{d}}_i,{\mathrm{\ρ}}_i)={\mathrm{\ρ}}_{i}N\left({\tilde{d}}_i^n\right|{\tilde{d}}_i,{\mathrm{\τ}}_i^2)+(1-{\mathrm{\ρ}}_i)U\left({\tilde{d}}_i^n\right|{\tilde{d}}_i^{\min }{\tilde{d}}_i^{\max })---(2-2)$

The implication of above formula is: key frame r_mMiddle i-th angle point is through image I_kTriangulation, if being effectively measure, then the inverse of measurement resultMeet normal distributionWhereinThe degree of depth inverse caused for one pixel error of the plane of delineation calculates variance, if being invalid measurement, thenMeet and be uniformly distributedWhereinMinima that the degree of depth set for the prior information according to current scene is reciprocal and maximum；Pass throughMeasurement update, the degree of depth is reciprocalPosterior probability distribution such as formula (2-3):

$C\×p\left({\tilde{d}}_i^n\right|{\tilde{d}}_i,{\mathrm{\ρ}}_i)q({\tilde{d}}_i,{\mathrm{\ρ}}_i|a_{n-1},b_{n-1},{\mathrm{\μ}}_{n-1},{\mathrm{\σ}}_{n-1}^2)---(2-3)$

Wherein, C is that guarantee probability is distributed normalized constant, by moment-matching method, calculates a_n, b_n, μ_n,

S24: reject and effectively measure probabilityToo low angle point, by uncertainty σ_nMeet the point required to add in map.

4. as claimed in claim 2 method, it is characterised in that described step 2) carry out estimation, map structuring and depth map parallel and estimate that the depth map of three threads estimates that thread specifically comprises the following steps that

If the new image obtained is chosen as new key frame at estimation thread step S12, from the key frame being newly added, extract edge point, perform step S31～S33；Otherwise utilize the pose of the estimation thread step S14 present frame obtained, perform step S34～S36, update the probability depth filter of edge point by new image information, and reject the edge point that effective observation probability is too low；Specifically comprise the following steps that

S31: in the key frame being newly added, utilizes Sobel operator to calculate the shade of gray G that image is horizontal and vertical_xAnd G_y, shade of gray figure usesApproximate；Selecting 2500 to 4000 pixels that shade of gray is maximum from image, when the significant pixel number of gradient is less than 2500, Grads threshold 450 is reduced to original 0.95 times, when number is more than 4000, increase threshold value is original 1.05 times；

S32: remember that new key frame is r_m, previous key frame is r_m-1, by frame r_m-1Depth map D^m-1Propagate new key frame depth maps D^m；r_m-1In, edge pointThe reciprocal average of the degree of depth beThe variance of degree of depth inverse isWherein,Represent the average that the degree of depth of edge point is reciprocal,Representing the variance that the degree of depth is reciprocal, subscript i represents frame r_m-1In i-th edge point；r_m-1To r_mRelative pose be T_{M, m-1}, edge pointAt frame r_mMiddle corresponding sides are along the position putCan be calculated by projection relation obtains such as formula (3-1):

$u_{e_i}^{\′}=\mathrm{\π}(T_{m,m-1}\·{\mathrm{\π}}^{-1}(u_{e_i},\frac{1}{{\widetilde{\mathrm{\μ}}}_{e_i}}\left)\right)---(3-1)$

NoteDegree of depth inverse beThe variance of degree of depth inverse is designated asThen:

${\widetilde{\mathrm{\μ}}}_{e_i}^{\′}\left({\widetilde{\mathrm{\μ}}}_{e_i}\right)={({\widetilde{\mathrm{\μ}}}_{e_i}^{-1}-t_z)}^{-1}---(3-2)$

Wherein, t_zRepresent camera translation on optical axis；Thus deriveThe reciprocal variance of the degree of depthFor:

${\widetilde{\mathrm{\σ}}}_{e_i}^{\′2}=J_{{\widetilde{\mathrm{\μ}}}_{e_i}}\·{\widetilde{\mathrm{\σ}}}_{e_i}^2\·J_{{\widetilde{\mathrm{\μ}}}_{e_i}}^T+{\mathrm{\σ}}_p^2={\left(\frac{{\widetilde{\mathrm{\μ}}}_{e_i}^{\′}}{{\widetilde{\mathrm{\μ}}}_{e_i}}\right)}^4\·{\widetilde{\mathrm{\σ}}}_{e_i}^2+{\mathrm{\σ}}_p^2---(3-3)$

WhereinRepresent uncertainty in traffic, willIt is tied to r_mIn nearest pixel

S33: each the edge point for extracting in step S31 creates probability depth filter, if 3x3 pixel coverage internal memory is in prior estimate near some edge pointThen its degree of depthAverageInitialize such as formula (3-4):

${\mathrm{\μ}}_{e_i}=\frac{{\mathrm{\Σ}}_i\frac{1}{{\widetilde{\mathrm{\σ}}}_{e_i}^{\′2}}\frac{1}{{\widetilde{\mathrm{\μ}}}_{e_i}^{\′}}}{{\mathrm{\Σ}}_i\frac{1}{{\widetilde{\mathrm{\σ}}}_{e_i}^{\′2}}}---(3-4)$

The uncertainty that then the parameter degree of depth of probability depth filter is reciprocalThen it is initialized as in prior estimate minimum uncertaintyIf being absent from prior estimate, then probability depth filter is initialized as mean depth and maximum uncertainty；TakeThen illuminated (2-1) is pressed for each edge and constructs probability depth filter such as formula (3-5):

$q({\tilde{d}}_{e_i},{\mathrm{\ρ}}_i|a_n,b_n,{\mathrm{\μ}}_n,{\mathrm{\σ}}_n^2):=Beta\left({\mathrm{\ρ}}_i\right|a_n,b_n)N\left({\tilde{d}}_{e_i}\right|{\mathrm{\μ}}_n,{\mathrm{\σ}}_n^2)---(3-5)$

Wherein, the definition of formula (3-5) each parameter is identical with formula (2-1), the parameters of the probability depth filter of corresponding sides edge point, a_nAnd b_nInitial value be set to 10, μ_n, σ_nIt is initialized asWith

S34: by new key frame r_mIt is set to the reference frame of successive image, utilizes follow-up picture frame r_mIn depth map D_mCarry out measuring and update；Use image I_kTo D^mDuring renewal, the edge point that note is updated isAccording to I_kAnd r_mRelative pose, at I_kIn determine straight line, it is ensured that I_kIn withCorresponding pixelOccur on this straight line；If some shade of gray direction, edge and described rectilinear direction angle are less than threshold value, obtain through the Block-matching of sub-pixel precisionCoordinate after, calculate the degree of depth of described edge point with triangulation meterPerform step S35；If described angle is more than this threshold value, then do not search for, return step S34 and check next edge point；

S35: the degree of depth that step S34 is obtainedInverted isWherein, subscript i represents it is i-th edge point in current key frame, and i is positive integer, and subscript n represents that this is that the n-th to current edge point point depth filter measures and updates, and n is positive integer, the inverse to the degree of depthProbability distribution model such as formula (3-6):

$p\left({\tilde{d}}_{e_i}^n\right|{\tilde{d}}_{e_i},{\mathrm{\ρ}}_i)={\mathrm{\ρ}}_{i}N\left({\tilde{d}}_{e_i}^n\right|{\tilde{d}}_{e_i},{\mathrm{\τ}}_i^2)+(1-{\mathrm{\ρ}}_i)U\left({\tilde{d}}_{e_i}^n\right|{\tilde{d}}_{e_i}^{\min },{\tilde{d}}_{e_i}^{\max })---(3-6)$

The implication of above formula is: key frame r_mMiddle i-th edge point is through image I_kTriangulation, if being effectively measure, then the inverse of measurement resultMeet normal distributionIf being invalid measurement, thenMeet and be uniformly distributedWhereinMinima that the degree of depth set for the prior information according to current scene is reciprocal and maximum；Pass throughMeasurement update, the degree of depth is reciprocalPosterior probability distribution such as formula (2-3):

$C\×p\left({\tilde{d}}_{e_i}^n\right|{\tilde{d}}_{e_i},{\mathrm{\ρ}}_i)q({\tilde{d}}_{e_i},{\mathrm{\ρ}}_i|a_{n-1},b_{n-1},{\mathrm{\μ}}_{n-1},{\mathrm{\σ}}_{n-1}^2)---(2-3)$

A is calculated by moment-matching method_n, b_n, μ_n,

S36: reject effectively measure probability too low (Less than 0.1) edge point.