CN103727930B - A kind of laser range finder based on edge matching and camera relative pose scaling method - Google Patents

Abstract

The invention discloses a kind of laser range finder based on edge matching and camera relative pose scaling method <b>, its step of </b> first extracts the edge contour of laser range finder cloud data edge contour and camera image, set up the probability distribution at cloud data edge and the probability distribution of image border, then minimize the KL distance of two distributions, try to achieve the relative pose parameter of laser range finder and camera.The inventive method does not rely on specific environment structure, does not rely on the auxiliary items such as scaling board; Can on-line operation, the relative pose of immediate updating laser range finder and camera; The point cloud edge contour extracted and image border profile can be further used for other application such as environmental objects identification and location.

Description

A kind of laser range finder based on edge matching and camera relative pose scaling method

Technical field

The present invention relates to multi-sensor information fusion field, particularly relate to a kind of laser range finder based on edge matching and camera relative pose scaling method.

Background technology:

Traditional laser range finder and the general scaling board of camera relative pose scaling method realize, by the angle point of visual identity scaling board, set up the constraint that these angle points are positioned at scaling board plane in space, minimum error function represents rotation and the translation matrix (R, T) of relative pose to obtain.

Though separately there is certain methods not need scaling board, also need the ad hoc structure of environment to set up geometrical constraint.

These methods all rely on specific object, need to prepare especially, and are unfavorable for on-line operation.Based on this, the present invention proposes a kind of laser range finder based on edge matching and camera relative pose scaling method, described method extracts the edge line representing environment edge contour respectively from laser data and image, by minimizing the symmetrical KL distance of edge line distribution, calculate the relative pose of laser range finder and camera.

Summary of the invention:

The object of the invention is to overcome the deficiencies in the prior art, a kind of laser range finder based on edge matching and camera relative pose scaling method are provided.

Based on the laser range finder of edge matching and the step of camera relative pose scaling method as follows:

1) three-dimensional point cloud of surrounding environment is obtained by laser range finder, simultaneously by the image of this environment of collected by camera;

2) extract the edge contour of the some cloud that laser range finder collects, obtain the three-dimensional point set L representing three-dimensional edges ₃;

3) according to performance parameter and the error model of laser range finder, the probability distribution of three-dimensional edges point set is determined

4) extract the edge contour of camera image, obtain the pixel set C representing two-dimentional edge ₂;

5) according to performance parameter and the error model of camera, the probability distribution of two-dimentional edge pixel is determined

6) with one group of coordinate conversion matrix comprising rotation matrix R and translation matrix T to represent the relative pose of laser range finder and camera, by three-dimensional edges point set L ₃under projecting to camera coordinates system, obtain two-dimentional edge point set L ₂;

7) according to the probability distribution of three-dimensional edges point set two-dimentional edge point set L is determined with projection relation ₂probability distribution

8) two probability distribution two dimension edge point set L are calculated ₂probability distribution with the probability distribution of two-dimentional edge pixel between symmetrical KL distance, with R, T for parameter, to minimize symmetrical KL distance KL _lCfor optimum target, try to achieve optimum laser range finder and camera relative pose transition matrix (R, T).

Described step 2) be: a) for a unordered cloud, search for each radius quantity be less than within the scope of r around and be no more than all nearest neighbor points of n, obtain point set N, for N fit Plane G, with the subpoint position of point set N in plane G for independent variable, the distance of point set N to plane G is functional value, matching Binary quadratic functions f, obtain Hessian matrix H, calculate the eigenvalue λ of Hessian matrix H ₁and λ ₂, assuming that λ ₁> λ ₂if, λ ₁> thresh ₁and λ ₂< thresh ₂, thresh ₁, thresh ₂be respectively respective threshold, then think that the center of circle point of current search scope is marginal point in unordered some cloud; B) for putting cloud in order, i.e. depth map, utilizes Canny algorithm to extract edge point set.

The described method for point set N fit Plane G is: the average calculating point set N, obtains the center c of plane G _g; Calculate N ^tthe proper vector of N, its minimal eigenvalue characteristic of correspondence vector is the normal vector n of plane G _g; The center c of plane G _gwith normal vector n _gnamely one is illustrated through center c _g, normal vector is n _gplane.

Described matching Binary quadratic functions f the method obtaining Hessian matrix H are: for every bit q in point set N, assuming that N ^tanother two the eigenwert characteristic of correspondence vectors of N are respectively α _gand β _g, calculate one with x, y for independent variable, f _n(x, y) is the key-value pair of value,

$\left\{\begin{array}{c}x={(q-{\left(q-c_G\right)}^T{n}_G)}^T{\mathrm{\&alpha;}}_G\\ y={(q-{\left(q-c_G\right)}^T{n}_G)}^T{\mathrm{\&beta;}}_G\\ f_N(x,y)={(q-c_G)}^T{n}_G\end{array}\right.$

Final formation one group (x, y) is to f _nthe key assignments of (x, y) maps, and utilizes least square method to ask for Hessian matrix H

$H={\mathrm{argmin}}_H{\&Sigma;}_{(x,y,f_N(x,y\left)\right)}{\left(\right(x,y)H{\left(x,y\right)}^T-f_N(x,y\left)\right)}^2,$

F is expressed as

f＝(x，y)H(x，y) ^T。

Described step 3) in the probability distribution of three-dimensional edges point set for:

$P_{L_3}\left(x\right)=\underset{q}{\&Sigma;}X(x-q,{\mathrm{Cov}}_q)$

Wherein X represents Gaussian distribution, Cov _qfor the uncertain covariance matrix of a q, depend on sensor performance parameter and error model.

Described step 4) in extract camera image edge contour method be Canny algorithm.

Described step 5) in the probability distribution of two-dimentional edge pixel for:

$P_{C_2}\left(x\right)=\underset{O}{\&Sigma;}X(x-O,{\mathrm{Cov}}_O)$

Cov _ofor the uncertain covariance matrix of pixel O, depend on sensor performance parameter and error model; Wherein X is Gaussian distribution.

Described step 6) in by three-dimensional edges point set L ₃under projecting to camera coordinates system, method is: for three-dimensional edges point set L ₃in 1 q, the subpoint Q in the camera imaging plane of its correspondence is

$Q=\left[\begin{array}{ccc}0& -1& 0\\ 0& 0& -1\end{array}\right](Rq-T),$

Wherein R and T is respectively rotation matrix and translation matrix.

Described step 7) according to three-dimensional edges point set L ₃probability distribution two-dimentional edge point set L is determined with projection relation ₂probability distribution method be:

$P_{L_2}\left(x\right)=\underset{Q}{\&Sigma;}X(x-Q,{\mathrm{Cov}}_Q)$

Wherein

${\mathrm{Cov}}_Q=\left[\begin{array}{ccc}0& -1& 0\\ 0& 0& -1\end{array}\right]R{\mathrm{Cov}}_q{R}^T{\left[\begin{array}{ccc}0& -1& 0\\ 0& 0& -1\end{array}\right]}^T;$

Wherein X is Gaussian distribution, Cov _qand Cov _qbe respectively the uncertain covariance matrix of q and Q.

Described step 8) in probability distribution two dimension edge point set L ₂probability distribution with the probability distribution of two-dimentional edge pixel between symmetrical KL distance KL _lCcomputing method be:

${\mathrm{KL}}_{LC}=\underset{x}{\&Sigma;}{P}_{L_2}\left(x\right)\ln \frac{P_{L_2}\left(x\right)}{P_{C_2}\left(x\right)}+\underset{x}{\&Sigma;}{P}_{C_2}\left(x\right)\ln \frac{P_{C_2}\left(x\right)}{P_{L_2}\left(x\right)}$

Minimize this symmetrical KL distance, relative pose transition matrix (R, T) can be obtained.

The present invention compared with prior art, the beneficial effect had:

1. do not rely on specific environment structure, do not rely on the auxiliary items such as scaling board;

2. can on-line operation, the relative pose of immediate updating laser range finder and camera;

3. extracted some cloud edge contour and image border profile can be further used for other application such as environmental objects identification and location.

Accompanying drawing explanation

Fig. 1 is laser range finder based on edge matching and camera relative pose scaling method operation steps schematic diagram;

Fig. 2 is laser range finder based on edge matching and camera relative pose scaling method implementation result figure.

Embodiment

A kind of laser range finder based on edge matching of the present invention and camera relative pose scaling method, after demarcating, the some cloud of laser range finder collection can carry out corresponding accurately with the image of collected by camera.On the one hand, image can be the colouring of some cloud, obtains colour point clouds, or is that the surface mesh being summit with a cloud pastes color texture, obtain grain surface model; On the other hand, some cloud can the degree of depth of indicating section image-region, for the application such as identification, location based on image provides support.

Timing signal, allows laser range finder and camera image data simultaneously, and ensures that their observation scope major part overlaps, and the some cloud collect laser range finder and the image of collected by camera process, and obtain the relative pose of laser range finder and camera online.

As described in Figure 1, based on the laser range finder of edge matching and the step of camera relative pose scaling method as follows:

1) three-dimensional point cloud of surrounding environment is obtained by laser range finder, simultaneously by the image of this environment of collected by camera;

2) extract the edge contour of the some cloud that laser range finder collects, obtain the three-dimensional point set L representing three-dimensional edges ₃;

3) according to performance parameter and the error model of laser range finder, the probability distribution of three-dimensional edges point set is determined

4) extract the edge contour of camera image, obtain the pixel set C representing two-dimentional edge ₂;

5) according to performance parameter and the error model of camera, the probability distribution of two-dimentional edge pixel is determined

6) with one group of coordinate conversion matrix comprising rotation matrix R and translation matrix T to represent the relative pose of laser range finder and camera, by three-dimensional edges point set L ₃under projecting to camera coordinates system, obtain two-dimentional edge point set L ₂;

7) according to the probability distribution of three-dimensional edges point set two-dimentional edge point set L is determined with projection relation ₂probability distribution

8) two probability distribution two dimension edge point set L are calculated ₂probability distribution with the probability distribution of two-dimentional edge pixel between symmetrical KL distance, with R, T for parameter, to minimize symmetrical KL distance KL _lCfor optimum target, try to achieve optimum laser range finder and camera relative pose transition matrix (R, T).

Described step 2) be: a) for a unordered cloud, search for each radius quantity be less than within the scope of r around and be no more than all nearest neighbor points of n, obtain point set N, for N fit Plane G, with the subpoint position of point set N in plane G for independent variable, the distance of point set N to plane G is functional value, matching Binary quadratic functions f, obtain Hessian matrix H, calculate the eigenvalue λ of Hessian matrix H ₁and λ ₂, assuming that λ ₁> λ ₂if, λ ₁> thresh ₁and λ ₂< thresh ₂, thresh ₁, thresh ₂be respectively respective threshold, then think that the center of circle point of current search scope is marginal point in unordered some cloud; B) for putting cloud in order, i.e. depth map, utilize Canny algorithm (CannyJ.Acomputationalapproachtoedgedetection [J] .PatternAnalysisandMachineIntelligence, IEEETransactionson, 1986 (6): 679-698.) extract edge point set.

The described method for point set N fit Plane G is: the average calculating point set N, obtains the center c of plane G _g; Calculate N ^tthe proper vector of N, its minimal eigenvalue characteristic of correspondence vector is the normal vector n of plane G _g; The center c of plane G _gwith normal vector n _gnamely one is illustrated through center c _g, normal vector is n _gplane.

Described matching Binary quadratic functions f the method obtaining Hessian matrix H are: for every bit q in point set N, assuming that N ^tanother two the eigenwert characteristic of correspondence vectors of N are respectively α _gand β _g, calculate one with x, y for independent variable, f _n(x, y) is the key-value pair of value,

$\left\{\begin{array}{c}x={(q-{\left(q-c_G\right)}^T{n}_G)}^T{\mathrm{\&alpha;}}_G\\ y={(q-{\left(q-c_G\right)}^T{n}_G)}^T{\mathrm{\&beta;}}_G\\ f_N(x,y)={(q-c_G)}^T{n}_G\end{array}\right.$

Final formation one group (x, y) is to f _nthe key assignments of (x, y) maps, and utilizes least square method to ask for Hessian matrix H

$H={\mathrm{argmin}}_H{\&Sigma;}_{(x,y,f_N(x,y\left)\right)}{\left(\right(x,y)H{\left(x,y\right)}^T-f_N(x,y\left)\right)}^2,$

F is expressed as

f＝(x，y)H(x，y) ^T。

Described step 3) in the probability distribution of three-dimensional edges point set for:

$P_{L_3}\left(x\right)=\underset{q}{\&Sigma;}X(x-q,{\mathrm{Cov}}_q)$

Wherein X represents Gaussian distribution, Cov _qfor the uncertain covariance matrix of a q, depend on sensor performance parameter and error model (BaeKH, BeltonD, LichtiD.Aframeworkforpositionuncertaintyofunorganisedthr ee-dimensionalpointcloudsfromnear-monostaticlaserscanner susingcovarianceanalysis [C] //ProceedingsoftheISPRSWorkshop " Laserscanning.2005.).

Described step 4) in extract camera image edge contour method be Canny algorithm (CannyJ.Acomputationalapproachtoedgedetection [J] .PatternAnalysisandMachineIntelligence, IEEETransactionson, 1986 (6): 679-698.).

Described step 5) in the probability distribution of two-dimentional edge pixel for:

$P_{C_2}\left(x\right)=\underset{O}{\&Sigma;}X(x-O,{\mathrm{Cov}}_O)$

Cov _ofor the uncertain covariance matrix of pixel O, depend on sensor performance parameter and error model (DeSantoM, LiguoriC, PietrosantoA.Uncertaintycharacterizationinimage-basedmea surements:apreliminarydiscussion [J] .InstrumentationandMeasurement, IEEETransactionson, 2000,49 (5): 1101-1107.); Wherein X is Gaussian distribution.

Described step 6) in by three-dimensional edges point set L ₃under projecting to camera coordinates system, method is: for three-dimensional edges point set L ₃in 1 q, the subpoint Q in the camera imaging plane of its correspondence is

$Q=\left[\begin{array}{ccc}0& -1& 0\\ 0& 0& -1\end{array}\right](Rq-T),$

Wherein R and T is respectively rotation matrix and translation matrix.

Described step 7) according to three-dimensional edges point set L ₃probability distribution two-dimentional edge point set L is determined with projection relation ₂probability distribution method be:

$P_{L_2}\left(x\right)=\underset{Q}{\&Sigma;}X(x-Q,{\mathrm{Cov}}_Q)$

Wherein

${\mathrm{Cov}}_Q=\left[\begin{array}{ccc}0& -1& 0\\ 0& 0& -1\end{array}\right]R{\mathrm{Cov}}_q{R}^T{\left[\begin{array}{ccc}0& -1& 0\\ 0& 0& -1\end{array}\right]}^T;$

Wherein X is Gaussian distribution, Cov _qand Cov _qbe respectively the uncertain covariance matrix of q and Q.

Described step 8) in probability distribution two dimension edge point set L ₂probability distribution with the probability distribution of two-dimentional edge pixel between symmetrical KL distance KL _lCcomputing method be:

${\mathrm{KL}}_{LC}=\underset{x}{\&Sigma;}{P}_{L_2}\left(x\right)\ln \frac{P_{L_2}\left(x\right)}{P_{C_2}\left(x\right)}+\underset{x}{\&Sigma;}{P}_{C_2}\left(x\right)\ln \frac{P_{C_2}\left(x\right)}{P_{L_2}\left(x\right)}$

Minimize this symmetrical KL distance, relative pose transition matrix (R, T) can be obtained.

Claims (10)

1., based on laser range finder and the camera relative pose scaling method of edge matching, it is characterized in that its step is as follows:

1) three-dimensional point cloud of surrounding environment is obtained by laser range finder, simultaneously by the image of this environment of collected by camera;

2) extract the edge contour of the some cloud that laser range finder collects, obtain the three-dimensional point set L representing three-dimensional edges ₃;

3) according to performance parameter and the error model of laser range finder, the probability distribution of three-dimensional edges point set is determined

4) extract the edge contour of camera image, obtain the pixel set C representing two-dimentional edge ₂;

5) according to performance parameter and the error model of camera, the probability distribution of two-dimentional edge pixel is determined

6) with one group of coordinate conversion matrix comprising rotation matrix R and translation matrix T to represent the relative pose of laser range finder and camera, by three-dimensional edges point set L ₃under projecting to camera coordinates system, obtain two-dimentional edge point set L ₂;

7) according to the probability distribution of three-dimensional edges point set two-dimentional edge point set L is determined with projection relation ₂probability distribution

8) two-dimentional edge point set L is calculated ₂probability distribution with the probability distribution of two-dimentional edge pixel between symmetrical KL distance KL _lC, with R, T for parameter, to minimize symmetrical KL distance KL _lCfor optimum target, try to achieve optimum laser range finder and camera relative pose transition matrix (R, T).

2. the laser range finder based on edge matching according to claim 1 and camera relative pose scaling method, it is characterized in that described step 2) be: a) for unordered some cloud, search for each radius quantity be less than within the scope of r around and be no more than all nearest neighbor points of n, obtain point set N, for N fit Plane G, with the subpoint position of point set N in plane G for independent variable, the distance of point set N to plane G is functional value, matching Binary quadratic functions f, obtain Hessian matrix H, calculate the eigenvalue λ of Hessian matrix H ₁and λ ₂, assuming that λ ₁> λ ₂if, λ ₁> thresh ₁and λ ₂< thresh ₂, thresh ₁, thresh ₂be respectively respective threshold, then think that the center of circle point of current search scope is marginal point in unordered some cloud; B) for putting cloud in order, i.e. depth map, utilizes Canny algorithm to extract edge point set.

3. the laser range finder based on edge matching according to claim 2 and camera relative pose scaling method, is characterized in that, the described method for point set N fit Plane G is: the average calculating point set N, obtains the center c of plane G _g; Calculate N ^tthe proper vector of N, its minimal eigenvalue characteristic of correspondence vector is the normal vector n of plane G _g; The center c of plane G _gwith normal vector n _gnamely one is illustrated through center c _g, normal vector is n _gplane.

4. the laser range finder based on edge matching according to claim 3 and camera relative pose scaling method, is characterized in that, described matching Binary quadratic functions f the method obtaining Hessian matrix H are: for every bit q in point set N, assuming that N ^tanother two the eigenwert characteristic of correspondence vectors of N are respectively α _gand β _g, calculate one with x, y for independent variable, f _n(x, y) is the key-value pair of value,

$\left\{\begin{array}{c}x={(q-{(q-c_G)}^T{n}_G)}^T{\mathrm{\&alpha;}}_G\\ y={(q-{(q-c_G)}^T{n}_G)}^T{\mathrm{\&beta;}}_G\\ f_N(x,y)={(q-c_G)}^T{n}_G\end{array}\right.$

Final formation one group (x, y) is to f _nthe key assignments of (x, y) maps, and utilizes least square method to ask for Hessian matrix H

$H={\mathrm{argmin}}_H{\mathrm{\&Sigma;}}_{(x,y,f_N(x,y\left)\right)}{\left(\right(x,y)H{\left(x,y\right)}^T-f_N(x,y\left)\right)}^2,$

F is expressed as

f＝(x,y)H(x,y) ^T。

5. the laser range finder based on edge matching according to claim 4 and camera relative pose scaling method, is characterized in that, described step 3) in the probability distribution of three-dimensional edges point set for:

$P_{L_3}\left(x\right)=\underset{q}{\&Sigma;}X(x-q,{\mathrm{Cov}}_q)$

Wherein X represents Gaussian distribution, Cov _qfor the uncertain covariance matrix of a q, depend on sensor performance parameter and error model.

6. the laser range finder based on edge matching according to claim 1 and camera relative pose scaling method, is characterized in that, described step 4) in extract camera image edge contour method be Canny algorithm.

7. the laser range finder based on edge matching according to claim 1 and camera relative pose scaling method, is characterized in that, described step 5) in the probability distribution of two-dimentional edge pixel for:

$P_{C_2}\left(x\right)=\underset{o}{\&Sigma;}X(x-O,{\mathrm{Cov}}_o)$

Cov _ofor the uncertain covariance matrix of pixel O, depend on sensor performance parameter and error model; Wherein X is Gaussian distribution.

8. the laser range finder based on edge matching according to claim 1 and camera relative pose scaling method, is characterized in that, described step 6) in by three-dimensional edges point set L ₃under projecting to camera coordinates system, method is: for three-dimensional edges point set L ₃in 1 q, the subpoint Q in the camera imaging plane of its correspondence is

$Q=\left[\begin{array}{ccc}0& -1& 0\\ 0& 0& -1\end{array}\right](Rq-T),$

Wherein R and T is respectively rotation matrix and translation matrix.

9. the laser range finder based on edge matching according to claim 8 and camera relative pose scaling method, is characterized in that, described step 7) according to three-dimensional edges point set L ₃probability distribution two-dimentional edge point set L is determined with projection relation ₂probability distribution method be:

$P_{L_2}\left(x\right)=\underset{Q}{\&Sigma;}X(x-Q,{\mathrm{Cov}}_Q)$

Wherein

${\mathrm{Cov}}_Q=\left[\begin{array}{ccc}0& -1& 0\\ 0& 0& -1\end{array}\right]R{\mathrm{Cov}}_q{R}^T{\left[\begin{array}{ccc}0& -1& 0\\ 0& 0& -1\end{array}\right]}^T;$

Wherein X is Gaussian distribution, Cov _qand Cov _qbe respectively the uncertain covariance matrix of q and Q.

10. the laser range finder based on edge matching according to claim 1 and camera relative pose scaling method, is characterized in that, described step 8) in probability distribution two dimension edge point set L ₂probability distribution with the probability distribution of two-dimentional edge pixel between symmetrical KL distance KL _lCcomputing method be:

${\mathrm{KL}}_{LC}=\underset{x}{\&Sigma;}{P}_{L_2}\left(x\right)\ln \frac{P_{L_2}\left(x\right)}{P_{C_2}\left(x\right)}+\underset{x}{\&Sigma;}{P}_{C_2}\left(x\right)\ln \frac{P_{C_2}\left(x\right)}{P_{L_2}\left(x\right)}$

Minimize this symmetrical KL distance, relative pose transition matrix (R, T) can be obtained.