CN107871327A - The monocular camera pose estimation of feature based dotted line and optimization method and system - Google Patents
The monocular camera pose estimation of feature based dotted line and optimization method and system Download PDFInfo
- Publication number
- CN107871327A CN107871327A CN201710994863.9A CN201710994863A CN107871327A CN 107871327 A CN107871327 A CN 107871327A CN 201710994863 A CN201710994863 A CN 201710994863A CN 107871327 A CN107871327 A CN 107871327A
- Authority
- CN
- China
- Prior art keywords
- point
- constraint
- line
- cost function
- matrix
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Withdrawn
Links
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/77—Determining position or orientation of objects or cameras using statistical methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Theoretical Computer Science (AREA)
- Data Mining & Analysis (AREA)
- Mathematical Optimization (AREA)
- Mathematical Analysis (AREA)
- Pure & Applied Mathematics (AREA)
- Computational Mathematics (AREA)
- Computing Systems (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Algebra (AREA)
- Life Sciences & Earth Sciences (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Bioinformatics & Computational Biology (AREA)
- Evolutionary Biology (AREA)
- Probability & Statistics with Applications (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Image Processing (AREA)
Abstract
The invention discloses a kind of estimation of the monocular camera pose of feature based dotted line and optimization method and system, including step：S1 is based on point constraint and line constraint construction determined linear system, and establishes the relation of spin matrix and translation vector；The cost function that the son constrained according to a constraint and line constrains, total cost function is built using least square method, by introducing Kerry parameter expression, obtains final cost function, the spin matrix and translation vector of camera pose are solved using the final cost function；S2 introduces Sang Pu sum errors structure object function in a constraint and line constraint, using spin matrix obtained by step S1 and translation vector as initial value, optimizes spin matrix and translation vector using FNS iterators.The present invention obtains equipment to monocular photograph and photograph scene is unrestricted, suitable for the photograph of vehicle-mounted monocular camera, airborne monocular camera and hand-held monocular camera；It is also applied for the photograph of road scene, aviation scene and indoor scene.
Description
Technical field
The invention belongs to position immediately with map structuring technical field, more particularly to a kind of monocular of feature based dotted line
Camera pose estimates and optimization method and system.
Background technology
Vision measurement is one of important subject in instant positioning and map structuring technology.In recent years, vision measurement
Fully studied and be widely used in robot, autonomous driving, wearable computing and augmented reality, and at some for example
It is especially suitable under the particular case of the invalid interior of global positioning system and underwater environment.Wherein monocular vision measurement uses monocular
The vision photometer of video camera has broader practice prospect, less expensive, more compact, it is easier to calibration etc. the advantages of.Monocular
The most standardization program of vision measurement is to extract one group of characteristics of image first and match them in successive frame, then rebuilds increment 3D
Structure and optimal camera pose, being minimized finally by re-projection error improves structure and pose.Therefore, research camera pose is estimated
Meter and optimization, to positioning immediately and map structuring achievement important in inhibiting.
In general, existing camera pose algorithm for estimating can based on be divided into three classes using the type of feature：Base
Estimate in the camera pose of point, the camera pose estimation based on line, and the camera pose estimation of dotted line combination.
Correspondence of the camera pose estimation dependent on three-dimensional point to two-dimensional points based on point, because point feature can be by easily
Detect, and there is good efficiency and performance in many circumstances, therefore can be established using this feature in three dimensions
Point the pose of camera is estimated with the corresponding of the point in two-dimentional photograph and with this.The problem of this method is present is when system includes
During a large amount of observations, these methods can not just be applied or become very poorly efficient, and in low texture scene particularly artificial environment
Middle poor effect.
Camera pose estimation application suitable method detection line segment based on line, then matched to estimate the position of camera
Appearance.The problem of this method is present is that precision is not high, it is necessary to add other constraints and optimization to obtain more accurate result.And side
Method step complicated calculations efficiency is also very low, takes longer.
The camera pose of dotted line combination estimates that there is presently no being well solved related work is also very limited,
Most of method is by linear restriction and the algorithm integration based on point.The shortcomings that these methods is that they will be when feature is rare
Invalid.Even if solving extreme condition using synteny and coplanar constraint, this method is also using not for different situations
Same model, can not regard Points And lines as equipotential condition, not accomplish real Joint of Line and Dot, and is difficult to extension and is used for redundancy sight
Examine.
The content of the invention
Accomplish that real Joint of Line and Dot, feature based dotted line monocular camera pose is estimated it is an object of the invention to provide a kind of
Meter and optimization method and system.
The monocular camera pose estimation of feature based dotted line of the present invention and optimization method, including step：
S1 builds cost function based on the geometrical constraint of m characteristic point and n bar characteristic curves, and camera is solved using cost function
The spin matrix R and translation vector t of pose, wherein, m+n >=3；This step further comprises：
S101 establishes the point constraint of each characteristic point matrix form and each characteristic curve matrix form based on geometrical constraint respectively
Line constrains；
All point constraints of S102 simultaneous and line constraint construction determined linear system；
S103 solves determined linear system, obtains expression formulas of the t on R；
S104 builds total cost function F (R, t) according to least square method：
Wherein：fi(R, t) represents the sub- cost function of i-th point of constraint,gj(R, t) is represented
The sub- cost function of j-th of line constraint,pgci,1And pgci,2For in i-th point of constraint
Two separate son constraints；lgci,1And lgci,2Two separate son constraints in being constrained for j-th of line；
S105 introduces Kerry parametric description R, F (R, t) is converted into fourth order polynomial, obtains final cost function F
(s)；
S106 make F (s) each characteristic point and each characteristic curve are corresponded to respectively R resolution parameter matrix single order local derviation be 0, meter
Calculate stationary point；
S107 is appliedBase technology recovers spin matrix R and translation vector t automatically；
The spin matrix R and translation vector t that S2 obtains to step S1 are optimized；This step further comprises：
S201 constrains all points and line constraint representation isForm, ω=m+n, it represents conditional number；ψωBy
Observed value ηωForm, ηωIt is characterized the 2*2 two-dimensional coordinate matrixes of line two-end-point composition；X includes all elements from R and t；
S202 pairsIntroduce Sang Pu sum errors structure object function, using the step S1 R obtained and t as initial value, profit
X is repeatedly solved with FNS iterators, so as to the R and t after being optimized；
S203 carries out singular value decomposition to finely tune R to the R after optimization；
Expression formulas of the t that S204 is obtained according to the R after fine setting and step S103 on R, the t after calculation optimization.
Further, the geometrical constraint { PGC of each characteristic point matrix formiBe
Wherein, { PGCiRepresent i-th of three-dimensional feature point geometrical constraint；Represent i-th of three-dimensional feature point institute on phase plate plane
The v coordinate of corresponding two dimensional character point；piRepresent the three-dimensional coordinate of i-th of three-dimensional feature point；T is represented from world coordinate system to phase
The translation vector of machine coordinate system.
Further, the geometrical constraint { LGC of each characteristic curve matrix formjBe
Wherein, { LGCjRepresent j-th strip three-dimensional feature line geometrical constraint；Represent the three-dimensional vector of j-th strip three-dimensional feature line
One-dimensional element；Represent j-th strip three-dimensional feature line；K represents the internal reference matrix of camera；Represent j-th strip three-dimensional feature line
The third dimension element of the three-dimensional normal vector of the plane formed with origin；It is flat to represent that j-th strip three-dimensional feature line is formed with origin
The two-dimensional element of the three-dimensional normal vector in face；T represents to carry out transposition to matrix；T and R represents from world coordinate system to camera respectively
The translation vector and spin matrix of coordinate system.
Further, sub-step S105 is specially：
Use Kerry parametric description spin matrix
According to expression formulas of the t on R, t is expressed asForm；
Remove the coefficient in R and t expression formula, R and t expression formula is substituted into total cost function F (R, t), obtained most
Whole cost function F (s), is designated as
Wherein, s=1+b2+c2+d2；B, c, d are spin matrix R resolution parameter；U is coefficient matrix；ρ=[b2,bc,
bd,b,c2,cd,d2,d,1]T；H (s) is with unknown variable vector s=[b, c, d]TQuadratic polynomial.
Further, in step S202, the object function isWherein, H (x) represents mulberry
General sum error；To describe ψωProbabilistic covariance matrix.
The monocular camera pose estimation of feature based dotted line of the present invention and optimization system, including：
First module, for building cost function based on the geometrical constraint of m characteristic point and n bar characteristic curves, utilize cost
Function solves the spin matrix R and translation vector t of camera pose, wherein, m+n >=3；
First module further comprises submodule：
First submodule, for establishing the constraint of the point of each characteristic point matrix form and each characteristic curve respectively based on geometrical constraint
The line constraint of matrix form；
Second submodule, for all point constraints of simultaneous and line constraint construction determined linear system；
3rd submodule, for solving determined linear system, obtain expression formulas of the t on R；
4th submodule, for building total cost function F (R, t) according to least square method：
Wherein：fi(R, t) represents the sub- cost function of i-th point of constraint,gj
(R, t) represents the sub- cost function of j-th of line constraint,pgci,1And pgci,2For i-th
Two separate son constraints in individual point constraint；lgci,1And lgci,2For two separate sons in j-th of line constraint about
Beam；
5th submodule, for introducing Kerry parametric description R, F (R, t) is converted into fourth order polynomial, obtain final
Cost function F (s)；
6th submodule, for make F (s) each characteristic point and each characteristic curve are corresponded to respectively R resolution parameter matrix one
Rank local derviation is 0, calculates stationary point；
7th submodule, for applyingBase technology recovers spin matrix R and translation vector t automatically；
Second module, spin matrix R and translation vector t for being obtained to the first module are optimized；
Second module further comprises：
8th submodule, for being by all point constraints and line constraint representationForm, ω=m+n, it is represented
Conditional number；ψωBy observed value ηωForm, ηωIt is characterized the 2*2 two-dimensional coordinate matrixes of line two-end-point composition；X is included from R's and t
All elements；
9th submodule, for pairIntroduce Sang Pu sum errors structure object function, the R obtained with the first module
It is initial value with t, x is repeatedly solved using FNS iterators, so as to the R and t after being optimized；
Tenth submodule, for carrying out singular value decomposition to the R after optimization to finely tune R；
11st submodule, for the expression formula of the t that is obtained according to the R after fine setting and the 3rd submodule on R, calculate
T after optimization.
The invention has the advantages that and beneficial effect：
Geometrical constraint and reliable algebraic solver of the present invention by robust, directly retrieval is without initializing or repeatedly
All fixing points of the cost function minimized in the case of generation by First Order Optimality Condition, at the beginning of obtaining reliable camera pose
Initial value.And in order to improve camera pose, the present invention proposes a kind of new optimisation strategy, by considering the specific of each feature
Uncertainty come minimize without constraint Sampson errors, more reasonably to reduce influence of noise.In addition, the present invention is also by keeping away
Exempt from higher-dimension parameter search, make process simpler than traditional light-stream adjustment.The first model that the present invention carries can be located in equivalent
Manage Points And lines key element, this be applied to institute minimum number in need be 3 Points And lines key element all minimums, and have can
To be easily extended to that there is the advantages of various situations of more redundancy observations.
The inventive method to monocular photograph obtain equipment it is unrestricted, suitable for vehicle-mounted monocular camera, airborne monocular camera and
The photograph of hand-held monocular camera；It is also unrestricted to photograph scene, suitable for the phase of road scene, aviation scene and indoor scene
Piece.
Brief description of the drawings
Fig. 1 is the idiographic flow schematic diagram of the inventive method；
Fig. 2 is the principle schematic of geometrical constraint of the present invention.
Embodiment
Below in conjunction with the drawings and specific embodiments, technical solution of the present invention is further illustrated.
See Fig. 1, the monocular camera pose estimation of distinguished point based and characteristic curve of the present invention and optimization method, specific steps are such as
Under：
Step 1, the geometrical constraint of characteristic point and characteristic curve is established, and builds cost function according to this, is asked using cost function
Solve camera pose initial spin matrix and translation vector.
This step includes following sub-step：
Step 101, the geometrical constraint of characteristic point and characteristic curve matrix form is established, i.e. point constraint and line constraint.
By the geometrical constraint that the three point on a straight line between photograph and space and intersecting lens are coplanar, required point constraint can be constructed
Constrained with line.The present invention uses the principle of geometrical constraint to see Fig. 2, is described in figure of equal value using characteristic point and characteristic curve progress
The basic model of camera pose estimation, W and C represent the camera coordinates system of world coordinate system and Current camera respectively, and m three-dimensional special
Sign pointIts corresponding two dimensional character point on phase plate plane is designated asFor convenience of description, the present invention uses normalizing
Change coordinateTwo dimensional character point is described, wherein,WithRepresent the two-dimensional coordinate of characteristic point.Meanwhile n bars three
Dimensional feature lineIts corresponding two dimensional character line on phase plate plane is designated asπ in Fig. 2jRepresent three-dimensional feature line
LjThe plane formed with photo centre O.To make description form more succinct, the method for expressing of line segment uses homogeneous coordinates.With
Point feature is different, and three-dimensional-two-dimentional matching characteristic line that the present invention uses does not require that end points is consistent, i.e., does not require sj、With photography
Center O or ej、With photo centre's O three point on a straight line, wherein, sjAnd ejRepresent three-dimensional feature line LjEnd points,WithFor two dimension
Characteristic curveEnd points.
The geometry of the point constraint and line constraint is represented using matrix form, as follows：
To i-th of spatial point, i.e. i-th of three-dimensional feature point, structure point constraint { PGCi}：
Wherein, piThe three-dimensional coordinate of i-th of spatial point is represented,Represent the v coordinate of two-dimensional points corresponding to i-th of spatial point.
{ LGC is constrained to j-th of linej}：Wherein,Represent j-th strip three-dimensional feature
The one-dimensional element of the three-dimensional vector of line,J-th strip three-dimensional feature line is represented, 3 × 3 internal reference squares of camera known to K expressions
Battle array,The third dimension element of the three-dimensional normal vector for the plane that the origin of j-th strip three-dimensional feature line and camera coordinates system is formed is represented,Represent the two-dimensional element of the three-dimensional normal vector for the plane that the origin of j-th strip three-dimensional feature line and camera coordinates system is formed, T tables
Show and transposition is carried out to matrix.
In the present invention, t and R represent the translation vector and spin matrix from world coordinate system to camera coordinates system respectively.
Internal reference matrix K is as follows：
Internal reference matrix K includes 5 inner parameters, and the inner parameter includes focal length, image sensor size and principal point.Its
In, inner parameter αx=fmx, αy=fmy, αxAnd αyRepresent the focal length weighed with pixel, mxAnd myPixel size is represented respectively
To the scale factor of actual range, f is the real focal length weighed with distance.γ is represented between the x coordinate and y-coordinate of image plane
The coefficient of skewness, usually 0；u0And v0Represent the coordinate of the principal point of image plane.
Step 102, determined linear system is constructed according to the geometrical constraint of characteristic point and characteristic curve, i.e., in simultaneous step 101
All point constraints and line constraint construct unified equation group.
Step 103, expression formulas of the translation vector t on spin matrix R is solved according to the solution of determined linear system, will asked
Topic is converted into the problem of solving spin matrix R.
The solution of determined linear system is as follows：
To overdetermined equation At=b, wherein, A is coefficient matrix, and b is constant term.T can be solved according to equation below：
T=(ATA)-1ATb (2)
Bring determined linear system in step 102 into this formula (2), you can obtain expression formulas of the t on R, you can will simultaneously
T and R problems are asked to be converted into the problem of onlying demand R.
Step 104, according to constraint and the sub- cost function of line constraint definition, and total cost letter is built according to least square method
Number.
By a constraint { PGCiSeparately it is designated as pgci,1And pgci,2, point constraint { PGC is represented respectivelyiTwo independently of each other
Son constraint.Similarly, line constraint { LGCjLgc can be designated asi,1And lgci,2, line constraint { LGC is represented respectivelyjTwo mutually
Independent son constraint.
Point constraint and line constraint include three sons and constrained, and in this three son constraints, there is two separate son constraints,
Another sub- constraint is obtained according to this two separate son constraints.pgci,1And pgci,2And lgci,1And lgci,2Then
For the separate son constraint of two in three son constraints.
The sub- cost function of a constraint and line constraint can be then built respectively, it is as follows：
The point of the ith feature point constrains sub- cost function fi(R, t) is：
The line of the j-th strip characteristic curve constrains sub- cost function gj(R, t) is：
According to least square method, total cost function F (R, t) is obtained：
In formula (5)：Represent left side equation F (R, t) being defined as right side equation, and solve and make it that F (R, t) acquirements are minimum
T and R when value；M is an amount of constraint, and n is line amount of constraint, m+n >=3.
Step 105, spin matrix R is described using existing Kerry parametric method, converts total cost function F (R, t)
For fourth order polynomial, and permanent positive coefficient is removed, obtain final cost function.
Parameterized using Kerry, R is written as：
In formula (6), s=1+b2+c2+d2；B, c, d are spin matrix R resolution parameter.
Method handles the expression formula of t in then step 103 like this, can be rewritten asForm, wherein, U is by line
Property equation group t=(ATA)-1AT3 × 10 coefficient matrixes after the expression of b matrixings, its dimension is not by point feature and line number of features
Influence, remain 3 × 10, ρ=[b2,bc,bd,b,c2,cd,d2,d,1]T。
Above-mentioned R and t expression formula is brought into the total cost function F (R, t) of formula (5), and due to s=1+b2+c2+d2
Just, then the coefficient in R and t expression formula can remove perseverance, then obtain final cost function F (s)：
In formula (7), h (s) is that have unknown variable vector s=[b, c, d]TQuadratic polynomial.
Step 106, the fourth order polynomial (i.e. cost function F (s)) obtained under single order optimal conditions to step 105 is carried out
Minimize, even its first-order partial derivative is equal to zero, sees formula (8), obtain all stationary points, stationary point is the minimum point of candidate.
In formula (8)：
skRepresent spin matrix resolution parameter corresponding to k-th of point feature or line feature；
ξk(s) represent F (s) to skSeek the cubic polynomial obtained by local derviation.
ξk(s) unknown number b, c, d value are stationary point when being equal to zero.
By ξk(s) polynomial system formed can be written as V θ=0, wherein, V is by ξk(s) the system of linear equations square of composition
3 × 20 coefficient matrixes that array represents, its dimension are not influenceed by point feature and line number of features, remain 3 × 20, θ
=[b3,b2c,...,b,c3,c2d,...,c,d3,d2,d,1]T, it is the vector being made up of R resolution parameter.
Step 107, applyBase technology, recover initial spin matrix R and translation vector t automatically.
By above step 101~106, camera pose estimation problem has been converted to solve multivariable polynomial system
The problem of system, its solution can useBase technology solves, i.e., is solved in the method for matrix decomposition in multinomial
Unknown number.Due to each ξk(s) degree is 3, thus the maximum quantity that may solve of system for 27 (test result indicates that working as spy
When levying number more than 7,The solver of base will return to unique solution in general).In order to obtain s final result,
Should be picked out from all stationary points Hessian matrixes be positive definite part as candidate value, then pass through minimum re-projection residual error
Selected in multiple candidates optimal.Afterwards, solved t can be calculated by the expression formula of the t in step 105 automatically.
Step 2, the camera pose initial rotation vector R and translation vector t that are obtained in step 1 are optimized, to obtain
More accurate camera pose.
This step includes following sub-step：
Step 201, a constraint and line constraint are rewritten as new Unified Form：
, can be by { PGC when given ω=m+n groups point feature and line featureiAnd { LGCjIt is rewritten as unified form：
Wherein：
ψωBy observed value ηωComposition；ηωIt is a 2*2 being made up of two end points of characteristic curve two-dimensional coordinate matrix,
For each characteristic point and the two-end-point of every characteristic line detected, actual valueIt is defined as under noiseless status condition
Ideal value, its corresponding observed value η=[ηu,ηv]TIt can be considered that the plane coordinates that feature extraction algorithm is extracted adds zero-mean
Value after Gaussian noise Δ η disturbance, Δ η have known 2 × 2 covariance matrix Cη, subscript ω is known respective conditions
Number.
X is unknown 12 dimensional vector for including the whole element from R and t, i.e. x=[R1,R2,...,R9,t1,t2,t3]T,
Wherein R1,R2,...,R9Represent 9 elements of spin matrix, t1,t2,t3Represent three elements of translation vector.
By upper while also have：
In formula (10)：
η=ηωRepresent and work as variable η values as ηωWhen partial derivative value；
To describe obserred coordinate value ψωProbabilistic covariance matrix；
In the case where approximation meets Gaussian Profile,It is considered as the covariance square being delivered to by law of propagation of errors under ψ spaces
Battle array.
Step 202, establish that be adapted under FNS alternative manners being capable of convergent target rapidly by introducing Sang Pu sum errors
Function, and result is obtained by FNS iterators.
Work as ψωWhen being approximately Gaussian Profile, the formula for minimizing mahalanobis distance in ψ spaces is as follows：
In formula (11), ω=m+n represents the summation of point feature and line number of features,For ψωTrue value.
Eliminate constraint extra in upper equation using Lagrange's multiplier, problem can be reduced to minimize following without about
The problem of Shu Fangcheng：
In formula (12)：
H (x) is then the form of the Sampson errors introduced in formula (11), and it can be by carrying out most to x derivations
Smallization：
In formula (13), M and L are 12 × 12 matrixes for depending on x.
The alternative manner of basic numerical scheme (FNS) has been used in above formula to solve x, for less M-L features
The characteristic vector of value is the x of each iteration new explanation, and M and L should be by newest x renewals until convergence.
Although this method does not need specific initial value in theory, in order that iteration quickly restrains, system can be used
The output of one model initializes it.
Step 203, singular value decomposition is carried out to the result after iteration to finely tune the spin matrix R by optimization of gained.
Because this method does not consider R orthogonal or decisive constraint, it is therefore desirable to posteriority correction is carried out after iteration, is passed through
Singular value decomposition (SVD) fine setting gained solution R
Step 204, spin matrix R brings the t obtained in step 1 into R expression formula formula, obtaining by the flat of optimization
The amount of shifting to t.
When it is implemented, the inventive method can realize automatic running flow based on software engineering, modularization side can be also used
Formula realizes corresponding system.Therefore, the estimation of monocular camera pose and optimization for a kind of feature based dotted line that the present invention accordingly provides
System, including with lower module：
First module, for building cost function based on the geometrical constraint of m characteristic point and n bar characteristic curves, utilize cost
Function solves the spin matrix R and translation vector t of camera pose, wherein, m+n >=3；
First module further comprises submodule：
First submodule, for establishing the constraint of the point of each characteristic point matrix form and each characteristic curve respectively based on geometrical constraint
The line constraint of matrix form；
Second submodule, for all point constraints of simultaneous and line constraint construction determined linear system；
3rd submodule, for solving determined linear system, obtain expression formulas of the t on R；
4th submodule, for building total cost function F (R, t) according to least square method：
Wherein：fi(R, t) represents the sub- cost function of i-th point of constraint,gj
(R, t) represents the sub- cost function of j-th of line constraint,pgci,1And pgci,2For i-th
Two separate son constraints in individual point constraint；lgci,1And lgci,2For two separate sons in j-th of line constraint about
Beam；
5th submodule, for introducing Kerry parametric description R, F (R, t) is converted into fourth order polynomial, obtain final
Cost function F (s)；
6th submodule, for make F (s) each characteristic point and each characteristic curve are corresponded to respectively R resolution parameter matrix one
Rank local derviation is 0, calculates stationary point；
7th submodule, for applyingBase technology recovers spin matrix R and translation vector t automatically；
Second module, spin matrix R and translation vector t for being obtained to the first module are optimized；
Second module further comprises：
8th submodule, for being by all point constraints and line constraint representationForm, ω=m+n, it is represented
Conditional number；ψωBy observed value ηωForm, ηωIt is characterized the 2*2 two-dimensional coordinate matrixes of line two-end-point composition；X is included from R's and t
All elements；
9th submodule, for pairIntroduce Sang Pu sum errors structure object function, the R and t obtained with step S1
For initial value, x is repeatedly solved using FNS iterators, so as to the R and t after being optimized；
Tenth submodule, for carrying out singular value decomposition to the R after optimization to finely tune R；
11st submodule, for the expression formula of the t that is obtained according to the R after fine setting and the 3rd submodule on R, calculate
T after optimization.
It is emphasized that embodiment of the present invention is illustrative, rather than it is limited.Therefore present invention bag
Include and be not limited to embodiment described in embodiment, it is every by those skilled in the art's technique according to the invention scheme
The other embodiment drawn, also belongs to the scope of protection of the invention.
Claims (6)
1. the monocular camera pose estimation of feature based dotted line and optimization method, it is characterized in that, including step：
S1 builds cost function based on the geometrical constraint of m characteristic point and n bar characteristic curves, and camera pose is solved using cost function
Spin matrix R and translation vector t, wherein, m+n >=3；This step further comprises：
The point that S101 establishes each characteristic point matrix form based on geometrical constraint respectively is constrained with the line of each characteristic curve matrix form about
Beam；
All point constraints of S102 simultaneous and line constraint construction determined linear system；
S103 solves determined linear system, obtains expression formulas of the t on R；
S104 builds total cost function F (R, t) according to least square method：
Wherein：fi(R, t) represents the sub- cost function of i-th point of constraint,gj(R, t) is represented
The sub- cost function of j-th of line constraint,pgci,1And pgci,2For in i-th point of constraint
Two separate son constraints；lgci,1And lgci,2Two separate son constraints in being constrained for j-th of line；
S105 introduces Kerry parametric description R, F (R, t) is converted into fourth order polynomial, obtains final cost function F (s)；
S106 make F (s) each characteristic point and each characteristic curve are corresponded to respectively R resolution parameter matrix single order local derviation be 0, calculating is stayed
Point；
S107 is appliedBase technology recovers spin matrix R and translation vector t automatically；
The spin matrix R and translation vector t that S2 obtains to step S1 are optimized；This step further comprises：
S201 constrains all points and line constraint representation isForm, ω=m+n, it represents conditional number；ψωBy observing
Value ηωForm, ηωIt is characterized the 2*2 two-dimensional coordinate matrixes of line two-end-point composition；X includes all elements from R and t；
S202 pairsSang Pu sum errors structure object function is introduced, using the step S1 R obtained and t as initial value, utilizes FNS
Iterator repeatedly solves x, so as to the R and t after being optimized；
S203 carries out singular value decomposition to finely tune R to the R after optimization；
Expression formulas of the t that S204 is obtained according to the R after fine setting and step S103 on R, the t after calculation optimization.
2. the monocular camera pose estimation of feature based dotted line as claimed in claim 1 and optimization method, it is characterized in that：
Geometrical constraint { the PGC of each characteristic point matrix formiBeWherein, { PGCiTable
Show the geometrical constraint of i-th of three-dimensional feature point；Represent i-th of three-dimensional feature point two dimensional character corresponding on phase plate plane
The v coordinate of point；piRepresent the three-dimensional coordinate of i-th of three-dimensional feature point；T represents the translation from world coordinate system to camera coordinates system
Vector.
3. the monocular camera pose estimation of feature based dotted line as claimed in claim 1 and optimization method, it is characterized in that：
Geometrical constraint { the LGC of each characteristic curve matrix formjBeWherein, { LGCj}
Represent the geometrical constraint of j-th strip three-dimensional feature line；Represent the one-dimensional element of the three-dimensional vector of j-th strip three-dimensional feature line
Element；Represent j-th strip three-dimensional feature line；K represents the internal reference matrix of camera；Represent that j-th strip three-dimensional feature line is formed with origin
Plane three-dimensional normal vector third dimension element；Represent the 3D approach for the plane that j-th strip three-dimensional feature line is formed with origin
The two-dimensional element of vector；T represents to carry out transposition to matrix；T and R represents flat from world coordinate system to camera coordinates system respectively
Move vector sum spin matrix.
4. the monocular camera pose estimation of feature based dotted line as claimed in claim 1 and optimization method, it is characterized in that：
Further, sub-step S105 is specially：
Use Kerry parametric description spin matrix
According to expression formulas of the t on R, t is expressed asForm；
Remove the coefficient in R and t expression formula, R and t expression formula is substituted into total cost function F (R, t), obtained final
Cost function F (s), it is designated as
Wherein, s=1+b2+c2+d2；B, c, d are spin matrix R resolution parameter；U is coefficient matrix；
ρ=[b2,bc,bd,b,c2,cd,d2,d,1]T；H (s) is with unknown variable vector s=[b, c, d]TIt is secondary multinomial
Formula.
5. the monocular camera pose estimation of feature based dotted line as claimed in claim 1 and optimization method, it is characterized in that：
In step S202, the object function isWherein, H (x) represents Sang Pu sum errors；For
ψ is describedωProbabilistic covariance matrix.
6. the estimation of monocular camera pose and the optimization system of feature based dotted line, it is characterized in that, including：
First module, for building cost function based on the geometrical constraint of m characteristic point and n bar characteristic curves, utilize cost function
The spin matrix R and translation vector t of camera pose are solved, wherein, m+n >=3；
First module further comprises submodule：
First submodule, for establishing the constraint of the point of each characteristic point matrix form and each feature wire matrix respectively based on geometrical constraint
The line constraint of form；
Second submodule, for all point constraints of simultaneous and line constraint construction determined linear system；
3rd submodule, for solving determined linear system, obtain expression formulas of the t on R；
4th submodule, for building total cost function F (R, t) according to least square method：
<mrow>
<munder>
<mi>min</mi>
<mrow>
<mi>R</mi>
<mo>,</mo>
<mi>t</mi>
</mrow>
</munder>
<mi>F</mi>
<mrow>
<mo>(</mo>
<mi>R</mi>
<mo>,</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mover>
<mo>=</mo>
<mi>&Delta;</mi>
</mover>
<msubsup>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>0</mn>
</mrow>
<mi>m</mi>
</msubsup>
<msup>
<msub>
<mi>f</mi>
<mi>i</mi>
</msub>
<mn>2</mn>
</msup>
<mrow>
<mo>(</mo>
<mi>R</mi>
<mo>,</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msubsup>
<mo>&Sigma;</mo>
<mrow>
<mi>j</mi>
<mo>=</mo>
<mn>0</mn>
</mrow>
<mi>n</mi>
</msubsup>
<msubsup>
<mi>g</mi>
<mi>j</mi>
<mn>2</mn>
</msubsup>
<mrow>
<mo>(</mo>
<mi>R</mi>
<mo>,</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
Wherein：fi(R, t) represents the sub- cost function of i-th point of constraint,gj(R,t)
The sub- cost function of j-th of line constraint is represented,pgci,1And pgci,2For i-th point about
Two separate son constraints in beam；lgci,1And lgci,2Two separate son constraints in being constrained for j-th of line；
5th submodule, for introducing Kerry parametric description R, F (R, t) is converted into fourth order polynomial, obtain final generation
Valency function F (s)；
6th submodule, for making the single order of resolution parameter matrix that F (s) corresponds to R to each characteristic point and each characteristic curve respectively inclined
Lead as 0, calculate stationary point；
7th submodule, for applyingBase technology recovers spin matrix R and translation vector t automatically；
Second module, spin matrix R and translation vector t for being obtained to the first module are optimized；
Second module further comprises：
8th submodule, for being by all point constraints and line constraint representationForm, ω=m+n, it represents condition
Number；ψωBy observed value ηωForm, ηωIt is characterized the 2*2 two-dimensional coordinate matrixes of line two-end-point composition；X includes all from R and t
Element；
9th submodule, for pairIntroduce Sang Pu sum errors structure object function, the R and t obtained using the first module as
Initial value, x is repeatedly solved using FNS iterators, so as to the R and t after being optimized；
Tenth submodule, for carrying out singular value decomposition to the R after optimization to finely tune R；
11st submodule, for the expression formula of the t that is obtained according to the R after fine setting and the 3rd submodule on R, calculation optimization
T afterwards.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710994863.9A CN107871327A (en) | 2017-10-23 | 2017-10-23 | The monocular camera pose estimation of feature based dotted line and optimization method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710994863.9A CN107871327A (en) | 2017-10-23 | 2017-10-23 | The monocular camera pose estimation of feature based dotted line and optimization method and system |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN107871327A true CN107871327A (en) | 2018-04-03 |
Family
ID=61753076
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710994863.9A Withdrawn CN107871327A (en) | 2017-10-23 | 2017-10-23 | The monocular camera pose estimation of feature based dotted line and optimization method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107871327A (en) |
Cited By (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109493384A (en) * | 2018-09-20 | 2019-03-19 | 顺丰科技有限公司 | Camera position and orientation estimation method, system, equipment and storage medium |
| CN109764864A (en) * | 2019-01-16 | 2019-05-17 | 南京工程学院 | A kind of indoor UAV position and orientation acquisition methods and system based on color identification |
| CN109931925A (en) * | 2019-03-12 | 2019-06-25 | 中国人民解放军军事科学院国防科技创新研究院 | Space rolling satellite spin pose refinement estimation method based on sequence image axis |
| CN109978919A (en) * | 2019-03-22 | 2019-07-05 | 广州小鹏汽车科技有限公司 | A kind of vehicle positioning method and system based on monocular camera |
| CN110113509A (en) * | 2018-02-01 | 2019-08-09 | 晨星半导体股份有限公司 | Circuit and relevant signal processing method applied to display device |
| CN110375732A (en) * | 2019-07-22 | 2019-10-25 | 中国人民解放军国防科技大学 | Monocular camera pose measurement method based on inertial measurement unit and point line characteristics |
| CN110910453A (en) * | 2019-11-28 | 2020-03-24 | 魔视智能科技(上海)有限公司 | Vehicle pose estimation method and system based on non-overlapping view field multi-camera system |
| CN111504276A (en) * | 2020-04-30 | 2020-08-07 | 哈尔滨博觉科技有限公司 | Visual projection scale factor set-based joint target function multi-propeller attitude angle acquisition method |
| CN112418288A (en) * | 2020-11-17 | 2021-02-26 | 武汉大学 | GMS and motion detection-based dynamic vision SLAM method |
| CN113436249A (en) * | 2021-06-01 | 2021-09-24 | 中国人民解放军63628部队 | Rapid and stable monocular camera pose estimation algorithm |
| WO2022073172A1 (en) * | 2020-10-09 | 2022-04-14 | 浙江大学 | Global optimal robot vision localization method and apparatus based on point-line features |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103679142A (en) * | 2013-12-02 | 2014-03-26 | 宁波大学 | Target human body identification method based on spatial constraint |
| CN103759716A (en) * | 2014-01-14 | 2014-04-30 | 清华大学 | Dynamic target position and attitude measurement method based on monocular vision at tail end of mechanical arm |
| CN105096341A (en) * | 2015-07-27 | 2015-11-25 | 浙江大学 | Mobile robot pose estimation method based on trifocal tensor and key frame strategy |
| CN106709223A (en) * | 2015-07-29 | 2017-05-24 | 中国科学院沈阳自动化研究所 | Sampling inertial guidance-based visual IMU direction estimation method |
| CN107133960A (en) * | 2017-04-21 | 2017-09-05 | 武汉大学 | Image crack dividing method based on depth convolutional neural networks |
| CN107160395A (en) * | 2017-06-07 | 2017-09-15 | 中国人民解放军装甲兵工程学院 | Map constructing method and robot control system |
-
2017
- 2017-10-23 CN CN201710994863.9A patent/CN107871327A/en not_active Withdrawn
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103679142A (en) * | 2013-12-02 | 2014-03-26 | 宁波大学 | Target human body identification method based on spatial constraint |
| CN103759716A (en) * | 2014-01-14 | 2014-04-30 | 清华大学 | Dynamic target position and attitude measurement method based on monocular vision at tail end of mechanical arm |
| CN105096341A (en) * | 2015-07-27 | 2015-11-25 | 浙江大学 | Mobile robot pose estimation method based on trifocal tensor and key frame strategy |
| CN106709223A (en) * | 2015-07-29 | 2017-05-24 | 中国科学院沈阳自动化研究所 | Sampling inertial guidance-based visual IMU direction estimation method |
| CN107133960A (en) * | 2017-04-21 | 2017-09-05 | 武汉大学 | Image crack dividing method based on depth convolutional neural networks |
| CN107160395A (en) * | 2017-06-07 | 2017-09-15 | 中国人民解放军装甲兵工程学院 | Map constructing method and robot control system |
Non-Patent Citations (6)
| Title |
|---|
| HAOANG LI,AT EL.: ""Combining Points and Lines for Camera Pose Estimation and Optimization in Monocular Visual Odometry"", 《2017 IEEE/RSJ INTERNATIONAL CONFERENCE ON INTELLIGENT ROBOTS AND SYSTEMS》 * |
| R. GOMEZ-OJEDA,AT EL.: ""PL-SVO: Semi-direct monocular visual odometry by combining points and line segments"", 《IEEE/RSJ INTERNATIONAL CONFERENCE ON INTELLIGENT ROBOTS AND SYSTEMS》 * |
| S. RAMALINGAM,AT EL.: ""Pose estimation using both points and lines for geo-localization"", 《IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION》 * |
| WEI LIU,AT EL.: ""A novel method of camera pose estimation by parabolic motion"", 《SCIENCEDIRECT》 * |
| YONG LIU,AT EL.: ""An Automated Method to Calibrate Industrial Robot Joint Offset Using Virtual Line-based Single-point Constraint Approach"", 《RESEARCHGATE》 * |
| 朱尊尚: ""基于三维地形重建与匹配的飞行器视觉导航方法研究"", 《中国博士学位论文全文数据库 工程科技Ⅱ辑》 * |
Cited By (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110113509A (en) * | 2018-02-01 | 2019-08-09 | 晨星半导体股份有限公司 | Circuit and relevant signal processing method applied to display device |
| CN109493384A (en) * | 2018-09-20 | 2019-03-19 | 顺丰科技有限公司 | Camera position and orientation estimation method, system, equipment and storage medium |
| CN109493384B (en) * | 2018-09-20 | 2021-03-09 | 顺丰科技有限公司 | Camera pose estimation method, system, device and storage medium |
| CN109764864A (en) * | 2019-01-16 | 2019-05-17 | 南京工程学院 | A kind of indoor UAV position and orientation acquisition methods and system based on color identification |
| CN109764864B (en) * | 2019-01-16 | 2022-10-21 | 南京工程学院 | Color identification-based indoor unmanned aerial vehicle pose acquisition method and system |
| CN109931925A (en) * | 2019-03-12 | 2019-06-25 | 中国人民解放军军事科学院国防科技创新研究院 | Space rolling satellite spin pose refinement estimation method based on sequence image axis |
| CN109978919A (en) * | 2019-03-22 | 2019-07-05 | 广州小鹏汽车科技有限公司 | A kind of vehicle positioning method and system based on monocular camera |
| CN110375732A (en) * | 2019-07-22 | 2019-10-25 | 中国人民解放军国防科技大学 | Monocular camera pose measurement method based on inertial measurement unit and point line characteristics |
| CN110910453A (en) * | 2019-11-28 | 2020-03-24 | 魔视智能科技(上海)有限公司 | Vehicle pose estimation method and system based on non-overlapping view field multi-camera system |
| CN110910453B (en) * | 2019-11-28 | 2023-03-24 | 魔视智能科技(上海)有限公司 | Vehicle pose estimation method and system based on non-overlapping view field multi-camera system |
| CN111504276B (en) * | 2020-04-30 | 2022-04-19 | 哈尔滨博觉科技有限公司 | Visual projection scale factor set-based joint target function multi-propeller attitude angle acquisition method |
| CN111504276A (en) * | 2020-04-30 | 2020-08-07 | 哈尔滨博觉科技有限公司 | Visual projection scale factor set-based joint target function multi-propeller attitude angle acquisition method |
| WO2022073172A1 (en) * | 2020-10-09 | 2022-04-14 | 浙江大学 | Global optimal robot vision localization method and apparatus based on point-line features |
| US11964401B2 (en) | 2020-10-09 | 2024-04-23 | Zhejiang University | Robot globally optimal visual positioning method and device based on point-line features |
| CN112418288A (en) * | 2020-11-17 | 2021-02-26 | 武汉大学 | GMS and motion detection-based dynamic vision SLAM method |
| CN112418288B (en) * | 2020-11-17 | 2023-02-03 | 武汉大学 | GMS and motion detection-based dynamic vision SLAM method |
| CN113436249A (en) * | 2021-06-01 | 2021-09-24 | 中国人民解放军63628部队 | Rapid and stable monocular camera pose estimation algorithm |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107871327A (en) | The monocular camera pose estimation of feature based dotted line and optimization method and system | |
| CN107564061B (en) | Binocular vision mileage calculation method based on image gradient joint optimization | |
| CN105913489A (en) | Indoor three-dimensional scene reconstruction method employing plane characteristics | |
| CN104395932A (en) | Method for registering data | |
| CN110009674A (en) | Monocular image depth of field real-time computing technique based on unsupervised deep learning | |
| CN104166989B (en) | Rapid ICP method for two-dimensional laser radar point cloud matching | |
| CN105225240A (en) | The indoor orientation method that a kind of view-based access control model characteristic matching and shooting angle are estimated | |
| CN107063190A (en) | Towards the high-precision direct method estimating of pose of calibration area array cameras image | |
| CN104281148A (en) | Mobile robot autonomous navigation method based on binocular stereoscopic vision | |
| Habib et al. | Quaternion-based solutions for the single photo resection problem | |
| CN110595479B (en) | SLAM track evaluation method based on ICP algorithm | |
| CN109870106A (en) | A kind of building volume measurement method based on unmanned plane picture | |
| CN106595595B (en) | A kind of Indoor Robot orientation method based on depth transducer | |
| CN114234967B (en) | Six-foot robot positioning method based on multi-sensor fusion | |
| CN105678833A (en) | Point cloud geometrical data automatic splicing algorithm based on multi-view image three-dimensional modeling | |
| CN107463871A (en) | A kind of point cloud matching method based on corner characteristics weighting | |
| CN113155152B (en) | Camera and inertial sensor spatial relationship self-calibration method based on lie group filtering | |
| CN111553954B (en) | Online luminosity calibration method based on direct method monocular SLAM | |
| CN107330934B (en) | Low-dimensional cluster adjustment calculation method and system | |
| Qing et al. | Weighted total least squares for the visual localization of a planetary rover | |
| CN107845107A (en) | A kind of optimization method of perspective image conversion | |
| CN102663680B (en) | Geometric image correction method based on region feature | |
| Suzuki et al. | SLAM using ICP and graph optimization considering physical properties of environment | |
| Sabatta et al. | Vision-based path following using the 1D trifocal tensor | |
| Wadenbäck et al. | Recovering planar motion from homographies obtained using a 2.5-point solver for a polynomial system |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| WW01 | Invention patent application withdrawn after publication |
Application publication date: 20180403 |
|
| WW01 | Invention patent application withdrawn after publication |