CN108053445A - The RGB-D camera motion methods of estimation of Fusion Features - Google Patents
The RGB-D camera motion methods of estimation of Fusion Features Download PDFInfo
- Publication number
- CN108053445A CN108053445A CN201711297500.6A CN201711297500A CN108053445A CN 108053445 A CN108053445 A CN 108053445A CN 201711297500 A CN201711297500 A CN 201711297500A CN 108053445 A CN108053445 A CN 108053445A
- Authority
- CN
- China
- Prior art keywords
- mrow
- mtr
- mtd
- msub
- dimensional
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 31
- 230000004927 fusion Effects 0.000 title abstract description 8
- 238000005070 sampling Methods 0.000 claims abstract description 14
- 238000007476 Maximum Likelihood Methods 0.000 claims abstract description 7
- 239000011159 matrix material Substances 0.000 claims description 15
- 238000001514 detection method Methods 0.000 claims description 12
- 238000013076 uncertainty analysis Methods 0.000 claims description 12
- 238000006073 displacement reaction Methods 0.000 claims description 6
- 238000005457 optimization Methods 0.000 claims description 5
- 230000011218 segmentation Effects 0.000 claims description 5
- 230000002159 abnormal effect Effects 0.000 claims description 4
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 3
- 238000002474 experimental method Methods 0.000 claims description 3
- 238000012887 quadratic function Methods 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 238000005259 measurement Methods 0.000 abstract description 5
- 238000005286 illumination Methods 0.000 abstract description 4
- 239000000284 extract Substances 0.000 abstract description 2
- 238000010276 construction Methods 0.000 abstract 1
- 238000010586 diagram Methods 0.000 description 5
- 230000000007 visual effect Effects 0.000 description 4
- 238000004458 analytical method Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000000053 physical method Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
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/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/10—Segmentation; Edge detection
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
Landscapes
- Engineering & Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Multimedia (AREA)
- Image Analysis (AREA)
Abstract
The invention discloses a kind of RGB D camera motion methods of estimation of Fusion Features.The present invention extracts two-dimensional points and two-dimentional linear feature first in RGB image, and two dimensional character back projection is obtained three-dimensional feature by the depth information in D images.Then, by RGB measurement errors and the error of depth survey error structure three-dimensional point, the uncertainty of straight line is measured by calculating the tripleplane of two-dimentional line-sampling point with the mahalanobis distance of estimated 3 d-line.Finally, the three-dimensional point and 3 d-line characteristic matching pair of adjacent two frame are merged, using unascertained information, the movement of RGB D cameras is calculated by Maximum-likelihood estimation.The present invention has merged the linear feature insensitive to illumination variation, and the error model of reasonable construction system improves robustness and the accuracy of camera motion estimation.
Description
Technical Field
The invention belongs to the technical field of machine vision, and particularly relates to a feature fusion RGB-D camera motion estimation method.
Background
In recent years, with the rapid development of image processing technology and the emergence of various vision sensors, vision-based mobile robots have received increasing attention. Compare laser radar, millimeter wave radar etc. and vision sensor can acquire abundanter environmental information, can also reduce cost simultaneously. Visual Odometer (VO) is a device that estimates the course of motion of a camera or a body connected to it (e.g., an automobile, a human or a mobile robot, etc.) by means of a Visual sensor only. The method is a sub-problem of visual simultaneous localization and map creation (VSLAM), and is a core problem of realizing autonomous navigation of a mobile robot. Commonly used visual sensors include monocular cameras, binocular cameras, panoramic cameras and RGB-D cameras. RGB-D cameras, such as Microsoft Kinect, Primer Sense, Hua Shuo Xtion PRO Live, Australian, etc., have attracted considerable interest not only because of their light weight and low cost, but more importantly because of the simultaneous color and depth information that such cameras provide. Depth information may solve scale problems in monocular cameras. Physical measurement of an RGB-D camera saves a large amount of depth information computation process compared to a binocular camera.
At present, the mainstream camera motion estimation methods are roughly classified into feature point methods (such as SIFT, SURF, ORB, etc.) and direct methods. The method is sensitive to illumination change, the characteristic point method is easily affected by uneven distribution of characteristics, the robustness of the algorithm is poor, and the application requirement cannot be met.
Disclosure of Invention
The invention aims to solve the problems that the existing camera motion estimation method is easily influenced by illumination and large noise is caused by uneven feature distribution. A robust RGB-D camera motion estimation method based on point and line feature fusion is provided.
In order to achieve the technical purpose, the invention adopts the technical scheme that the RGB-D camera motion estimation method with feature fusion comprises the following steps:
step one, extracting two-dimensional point and straight line features: on an RGB picture, extracting two-dimensional feature points and two-dimensional feature straight lines by using a feature point detection algorithm and a straight line segmentation detection algorithm respectively;
step two, performing feature back projection to the three dimensions and performing uncertainty analysis: combining depth information, utilizing a pinhole camera model to perform back projection on the extracted two-dimensional feature points and two-dimensional feature straight lines to three dimensions, and performing uncertainty analysis on the three-dimensional points and the three-dimensional straight lines under the assumption of Gaussian noise distribution;
step three, feature matching: for the point characteristics, calculating a characteristic point detection algorithm descriptor and matching the point characteristics of two continuous frames; calculating and matching the mean standard deviation descriptors of the linear features; then removing mismatching by using a random sampling consistency algorithm;
step four, estimating the motion of the camera: carrying out maximum likelihood estimation on the camera motion by using the uncertainty information; and solving the objective function of the problem through a Levenberg algorithm to obtain the pose of the camera.
The RGB-D camera motion estimation method with feature fusion comprises the following steps:
for one RGB image, two-dimensional feature points are obtained through a feature point detection algorithm, meanwhile, two-dimensional feature straight lines are obtained through a straight line segmentation detection algorithm, and a feature set { p is obtainedi,lj1,2, …, j 1,2, … }, where the two-dimensional point p isi＝[ui,vi]TTwo-dimensional straight line[ui,vi]TRepresents a point piPixel coordinates of ajAnd bjIs a straight line ljTwo end points of (a).
The second step of the feature-fused RGB-D camera motion estimation method comprises the following steps:
step 1, three-dimensional point characteristic and uncertainty analysis:
and (3) performing back projection on the two-dimensional point P in the image to obtain a three-dimensional point P through a pinhole camera model:
where (u, v) represents the pixel coordinate corresponding to the two-dimensional point p, d is the depth value corresponding to the two-dimensional point p, [ cu,cv]TIs the center of the aperture of the camera, fcIs the focal length;
the noise at two-dimensional point p is the mean 0 and the covarianceWith the noise of the depth value d of the two-dimensional point p being a quadratic function of the measured value, i.e. deltad＝c1d2+c2d+c3Constant coefficient c1、c2And c3The uncertainty of the three-dimensional point is obtained through experiments as follows:
wherein the covariance matrix of the noiseJacobi matrix
Wherein, I2Is a 2-dimensional unit matrix, δ is the noise variance;
step 2, three-dimensional straight line characteristic and uncertainty analysis:
sampling enough points of the two-dimensional straight line, discarding points with abnormal depth values, and calculating three-dimensional coordinates of the remaining points according to a formula (1); then adopting a random sampling consistency algorithm to resist and remove the local outer points which appear after the two-dimensional straight line is back projected to the three-dimensional due to the influence of depth noise, and obtaining a fitted three-dimensional straight line equation; with L ═ AT,BT]TTo represent a corresponding three-dimensional straight line, wherein AT,BTAre two three-dimensional points on a three-dimensional straight line;
measuring the uncertainty of the three-dimensional straight line by calculating the Mahalanobis distance between the three-dimensional point corresponding to the two-dimensional sampling points and the estimated three-dimensional straight line L;
three-dimensional point P ═ x, y, z]TTo a three-dimensional line L ═ AT,BT]TThe mahalanobis distance of (a) is defined as follows:
wherein Q epsilon L is an arbitrary point on the straight line L; with Q ═ a + λ (B-a), the optimal estimate of Q is then Q*,
Definition of
δ(P-L)＝P-Q*(4)
The mahalanobis distance from the three-dimensional point P to the three-dimensional line L is
The three-dimensional straight line is processed by a group of three-dimensional points { P after being processed by a random sampling consistency algorithmi,i＝1,…nLIs formed of, wherein P1Andrespectively representing two end points of a straight line, and making the error function of a three-dimensional straight line be
Maximum likelihood estimation L of three-dimensional straight line L*Equivalent to minimizing the following:
wherein∑PAnd delta (P-L) are respectively calculated by a formula (2) and a formula (4); solving the nonlinear minimization problem of equation (7) using the Levenberg algorithm with an optimized uncertainty of the three-dimensional line of
Wherein,
the feature-fused RGB-D camera motion estimation method comprises the following steps:
for the point features, SURF descriptors of the point features are calculated, and the point features of two adjacent frames of images are matched through the similarity between the descriptors; for straight line features, a mean normalized descriptor is calculated and then matched.
The feature-fused RGB-D camera motion estimation method comprises the following four steps:
t represents the motion conversion of two adjacent frames, and comprises T (X), RX + T, wherein R is a rotation matrix, T is a displacement vector, and a six-dimensional vector ξ is formed by taking the non-0 items of the rotation matrix and the displacement vector;
to be provided withAs a set of matched three-dimensional point sets of two adjacent frames F and F',then the three-dimensional straight line feature matching pairs of two adjacent frames are obtained; if three-dimensional point matching pairsThe corresponding real physical point is X under the local coordinate of the previous frameiThree-dimensional points, matching pairs in three-dimensional linesCorresponding to a true physical straight line of YjThen, the variable to be optimized in the camera odometer system is
The error function defining the system includes errors for point features and errors for straight line features, i.e.
Wherein the error of the point feature is
Error of straight line feature is
Error of a single straight line is η (L)i-Yi)＝[δ(Ai,Yi)T,δ(Bi,Yi)T]And is and
setting the target to find the variable Δ that minimizes the system error of the camera odometer system, i.e.
Wherein, sigmaf＝diag(ΣP,ΣL) Solving the above formula (10) by using a Levenberg algorithm to obtain an initial pose of the camera motion; and constructing a pose graph by using initial pose information of camera motion, and optimizing the motion track of the camera by using the pose graph at the back end of the SLAM through a graph optimization method.
In the RGB-D camera motion estimation method using feature fusion, in the step 2, the point with an abnormal depth value is discarded as a point with a null depth value.
The invention has the technical effects that:
(1) the linear characteristic is insensitive to illumination change, and the method combines the point characteristic and the line characteristic, thereby greatly improving the robustness of motion estimation. Under the indoor environment, the linear features are particularly rich, the linear features in the environment are very easy to extract, and the complexity of the algorithm is reduced.
(2) The invention carries out uncertainty analysis on the three-dimensional points and the three-dimensional straight lines, carries out maximum likelihood estimation on the camera motion by using uncertain information, obtains the camera pose through optimization, and provides an effective and reasonable initial value for back-end optimization.
Drawings
FIG. 1 is a block diagram of a feature fused RGB-D camera motion estimation system.
FIG. 2 is an exemplary diagram of a two-dimensional straight-line backprojection into three-dimensional space.
Fig. 3 is a pose diagram of the camera.
Detailed Description
Embodiments of the present invention will be further described with reference to the accompanying drawings.
The invention provides an RGB-D camera motion estimation method based on point and line feature fusion, a whole system block diagram is shown as an attached figure 1, and the method comprises the following steps:
s1, two-dimensional feature extraction: the system respectively extracts two-dimensional point features and two-dimensional straight line features by using the input RGB image.
In the invention, a group of two-dimensional point features are obtained by SURF (speeded up robust feature) feature point detection algorithm, and two-dimensional line features are obtained by LSD (line Segment detector) line segmentation detection algorithm. For an RGB map, its feature set { p }i,lj1,2, …, j 1,2, … }, where the two-dimensional point p isi＝[ui,vi]TTwo-dimensional straight line[ui,vi]TRepresents a point piPixel coordinates of ajAnd bjIs a straight line ljTwo end points of (a).
S2, three-dimensional feature acquisition and uncertainty analysis: and back projecting the two-dimensional features to three dimensions, respectively carrying out uncertain analysis on the three-dimensional points and the three-dimensional straight lines, and optimizing the estimation of the three-dimensional features.
(1) Three-dimensional point feature and uncertainty analysis
Two-dimensional point features are back-projected to three dimensions through a pinhole camera model. Assuming that a two-dimensional point P in an image corresponds to a depth value d, its corresponding three-dimensional point P is as follows:
here [ c ]u,cv]TIs the center of the aperture of the camera, fcIs the focal length.
By formula (1), the two-dimensional point feature can be back projected to a three-dimensional space to obtain a three-dimensional point feature (as shown in fig. 2). The RGB-D camera has larger measurement errors, and the noise of the acquired three-dimensional point cloud mainly comes from the RGB measurement errors and the depth value measurement errors.
Suppose that the noise at two-dimensional point p is mean 0 and covarianceA gaussian distribution of (a). Wherein, I2Is a 2-dimensional identity matrix and δ is the noise variance. The noise of the depth value d of the three-dimensional point P is quadratic in relation to the measured value, i.e. deltad＝c1d2+c2d+c3Constant coefficient c1、c2And c3Obtained through experiments. Assuming that the measurement error of the two-dimensional point p and the error of the depth value d are independent of each other, the uncertainty of the three-dimensional point is:
wherein the covariance matrix of the noiseJacobi matrix
(2) Three-dimensional straight line feature and uncertainty analysis
The projection of a straight line in three-dimensional space to two dimensions still retains the properties of a straight line. Therefore, the present invention first detects straight lines in a two-dimensional RGB map and then back-projects to a three-dimensional space. The idea of back projection is to sample a two-dimensional straight line with enough points, calculate the three-dimensional coordinates of these points according to equation (1), and discard those points with invalid depth values, as shown in fig. 2. Due to the influence of depth noise, local points (such as small circles in fig. 2) appear after a two-dimensional straight line is back-projected to three dimensions, and a random sampling consensus algorithm (RANSAC) is adopted to remove the local points and obtain a fitted three-dimensional straight line equation. We use L ═ AT,BT]TTo represent a corresponding three-dimensional straight line, wherein AT,BTAre two three-dimensional points on a three-dimensional straight line.
And measuring the uncertainty of the straight line by calculating the mahalanobis distance between the three-dimensional point corresponding to the two-dimensional sampling points and the estimated three-dimensional straight line L.
Three-dimensional point P ═ x, y, z]TTo a three-dimensional line L ═ AT,BT]TThe mahalanobis distance of (a) is defined as follows:
wherein Q ∈ L is an arbitrary point on the straight line L. Assuming Q is a + λ (B-a), then the minimization problem of equation (3) is equivalent to the univariate quadratic function minimization problem. The optimal estimation of Q can be obtained through derivation and calculationQ*I.e. by
Definition of
δ(P-L)＝P-Q*(4)
Thus, the mahalanobis distance of the three-dimensional point P to the three-dimensional straight line L can be written as
The three-dimensional straight line is processed by a group of three-dimensional points { P after being processed by a random sampling consistency algorithmi,i＝1,…nLIs formed of, wherein P1Andrespectively representing the two end points of the straight line. Let the error function of the three-dimensional straight line be
Maximum likelihood estimation L of three-dimensional straight line L*Equivalent to minimizing the following:
wherein∑PAnd delta (P-L) are respectively calculated by a formula (2) and a formula (4); the non-linear minimization problem of equation (7) is solved using the Levenberg algorithm. Uncertainty of the optimized three-dimensional straight line is
Wherein,
s3, feature matching of two adjacent frames: including point feature matching and straight line feature matching.
For point features, the invention calculates SURF descriptors thereof, and matches the point features of two adjacent frames of images through the similarity between the descriptors. For straight line features, MSLD descriptors are computed and then matched.
S4, motion estimation: the invention estimates the motion pose of the camera by using the point characteristics and the straight line characteristics.
The motion transformation of two adjacent frames is represented by T (X) ═ RX + T, where R is the rotation matrix and T is the displacement vector, and the non-0 terms of the rotation matrix and the displacement vector are taken to form a six-dimensional vector ξ.
Suppose thatAs a set of matched three-dimensional point sets of two adjacent frames F and F',then it is a matching pair of three-dimensional straight line features of two adjacent frames. If three-dimensional point matching pairsThe corresponding real physical point is X under the local coordinate of the previous frameiThree-dimensional points, matching pairs in three-dimensional linesCorresponding to a true physical straight line of YjThen, the variable to be optimized in the camera odometer system is
The error function defining the system includes errors for point features and errors for straight line features, i.e.
Wherein the error of the point feature is
Error of straight line feature is
Error of a single straight line is η (L)i-Yi)＝[δ(Ai,Yi)T,δ(Bi,Yi)T]And is and
setting the target to find the variable Δ that minimizes the system error of the camera odometer system, i.e.
Wherein, sigmaf＝diag(ΣP,ΣL). The invention solves the above formula (10) by using LM algorithm to obtain the pose of the camera motion. The pose diagram of the camera is shown in fig. 3. The estimated information of the initial pose of the camera can construct a pose graph, and the track of the camera is optimized through a graph optimization method.
Claims (6)
1. A feature-fused RGB-D camera motion estimation method is characterized by comprising the following steps:
step one, extracting two-dimensional point and straight line features: on an RGB picture, extracting two-dimensional feature points and two-dimensional feature straight lines by using a feature point detection algorithm and a straight line segmentation detection algorithm respectively;
step two, performing feature back projection to the three dimensions and performing uncertainty analysis: combining depth information, utilizing a pinhole camera model to perform back projection on the extracted two-dimensional feature points and two-dimensional feature straight lines to three dimensions, and performing uncertainty analysis on the three-dimensional points and the three-dimensional straight lines under the assumption of Gaussian noise distribution;
step three, feature matching: for the point characteristics, calculating a characteristic point detection algorithm descriptor and matching the point characteristics of two continuous frames; calculating and matching the mean standard deviation descriptors of the linear features; then removing mismatching by using a random sampling consistency algorithm;
step four, estimating the motion of the camera: carrying out maximum likelihood estimation on the camera motion by using the uncertainty information; and solving the objective function of the problem through a Levenberg algorithm to obtain the pose of the camera.
2. The method as claimed in claim 1, wherein the step one comprises the steps of:
for one RGB image, two-dimensional feature points are obtained through a feature point detection algorithm, meanwhile, two-dimensional feature straight lines are obtained through a straight line segmentation detection algorithm, and a feature set { p is obtainedi,lj1,2, …, j 1,2, … }, where the two-dimensional point p isi＝[ui,vi]TTwo-dimensional straight line[ui,vi]TRepresents a point piPixel coordinates of ajAnd bjIs a straight line ljTwo end points of (a).
3. The feature-fused RGB-D camera motion estimation method as claimed in claim 1, wherein the second step comprises the steps of:
step 1, three-dimensional point characteristic and uncertainty analysis:
and (3) performing back projection on the two-dimensional point P in the image to obtain a three-dimensional point P through a pinhole camera model:
<mrow> <mi>P</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mi>x</mi> </mtd> </mtr> <mtr> <mtd> <mi>y</mi> </mtd> </mtr> <mtr> <mtd> <mi>z</mi> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mo>(</mo> <mi>u</mi> <mo>-</mo> <msub> <mi>c</mi> <mi>u</mi> </msub> <mo>)</mo> <mi>d</mi> <mo>/</mo> <msub> <mi>f</mi> <mi>c</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>(</mo> <mi>v</mi> <mo>-</mo> <msub> <mi>c</mi> <mi>v</mi> </msub> <mo>)</mo> <mi>d</mi> <mo>/</mo> <msub> <mi>f</mi> <mi>c</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mi>d</mi> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>
where (u, v) represents the pixel coordinate corresponding to the two-dimensional point p, d is the depth value corresponding to the two-dimensional point p, [ cu,cv]TIs the center of the aperture of the camera, fcIs the focal length;
the noise at two-dimensional point p is the mean 0 and the covarianceWith the noise of the depth value d of the two-dimensional point p being a quadratic function of the measured value, i.e. deltad＝c1d2+c2d+c3Constant coefficient c1、c2And c3The uncertainty of the three-dimensional point is obtained through experiments as follows:
<mrow> <msub> <mo>&Sigma;</mo> <mi>P</mi> </msub> <mo>=</mo> <msub> <mi>J</mi> <mi>P</mi> </msub> <mi>c</mi> <mi>o</mi> <mi>v</mi> <mrow> <mo>(</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mi>p</mi> </mtd> </mtr> <mtr> <mtd> <mi>d</mi> </mtd> </mtr> </mtable> </mfenced> <mo>)</mo> </mrow> <msubsup> <mi>J</mi> <mi>P</mi> <mi>T</mi> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>
wherein the covariance matrix of the noiseJacobi matrix
Wherein, I2Is a 2-dimensional unit matrix, δ is the noise variance;
step 2, three-dimensional straight line characteristic and uncertainty analysis:
sampling enough points of the two-dimensional straight line, discarding points with abnormal depth values, and calculating three-dimensional coordinates of the remaining points according to a formula (1); then adopting a random sampling consistency algorithm to resist and remove the local outer points which appear after the two-dimensional straight line is back projected to the three-dimensional due to the influence of depth noise, and obtaining a fitted three-dimensional straight line equation; with L ═ AT,BT]TTo represent a corresponding three-dimensional straight line, wherein AT,BTAre two three-dimensional points on a three-dimensional straight line;
measuring the uncertainty of the three-dimensional straight line by calculating the Mahalanobis distance between the three-dimensional point corresponding to the two-dimensional sampling points and the estimated three-dimensional straight line L;
three-dimensional point P ═ x, y, z]TTo a three-dimensional line L ═ AT,BT]TThe mahalanobis distance of (a) is defined as follows:
<mrow> <msub> <mi>d</mi> <mrow> <mi>M</mi> <mi>A</mi> <mi>H</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>P</mi> <mo>,</mo> <mi>L</mi> <mo>)</mo> </mrow> <mo>=</mo> <munder> <mrow> <mi>m</mi> <mi>i</mi> <mi>n</mi> </mrow> <mrow> <mi>Q</mi> <mo>&Element;</mo> <mi>L</mi> </mrow> </munder> <msqrt> <mrow> <msup> <mrow> <mo>(</mo> <mi>P</mi> <mo>-</mo> <mi>Q</mi> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msubsup> <mo>&Sigma;</mo> <mi>P</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mrow> <mo>(</mo> <mi>P</mi> <mo>-</mo> <mi>Q</mi> <mo>)</mo> </mrow> </mrow> </msqrt> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow>
wherein Q epsilon L is an arbitrary point on the straight line L; with Q ═ a + λ (B-a), the optimal estimate of Q is then Q*,
Definition of
δ(P-L)＝P-Q*(4)
The mahalanobis distance from the three-dimensional point P to the three-dimensional line L is
<mrow> <msub> <mi>d</mi> <mrow> <mi>M</mi> <mi>A</mi> <mi>H</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>P</mi> <mo>,</mo> <mi>L</mi> <mo>)</mo> </mrow> <mo>=</mo> <msqrt> <mrow> <mi>&delta;</mi> <msup> <mrow> <mo>(</mo> <mi>P</mi> <mo>-</mo> <mi>L</mi> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msubsup> <mo>&Sigma;</mo> <mi>P</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mi>&delta;</mi> <mrow> <mo>(</mo> <mi>P</mi> <mo>-</mo> <mi>L</mi> <mo>)</mo> </mrow> </mrow> </msqrt> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow>
The three-dimensional straight line is processed by a group of three-dimensional points { P after being processed by a random sampling consistency algorithmi,i＝1,…nLIs formed of, wherein P1Andrespectively representing two end points of a straight line, and making the error function of a three-dimensional straight line be
<mrow> <mi>w</mi> <mrow> <mo>(</mo> <mi>L</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>P</mi> <mn>1</mn> </msub> <mo>-</mo> <mi>A</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&delta;</mi> <mrow> <mo>(</mo> <msub> <mi>P</mi> <mn>2</mn> </msub> <mo>,</mo> <mi>L</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&delta;</mi> <mrow> <mo>(</mo> <msub> <mi>P</mi> <mrow> <msub> <mi>n</mi> <mi>L</mi> </msub> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <mi>L</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>P</mi> <msub> <mi>n</mi> <mi>L</mi> </msub> </msub> <mo>-</mo> <mi>A</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow>
Maximum likelihood estimation L of three-dimensional straight line L*Equivalent to minimizing the following:
<mrow> <msup> <mi>L</mi> <mo>*</mo> </msup> <mo>=</mo> <munder> <mi>min</mi> <mi>L</mi> </munder> <mi>w</mi> <msup> <mrow> <mo>(</mo> <mi>L</mi> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msubsup> <mo>&Sigma;</mo> <mi>w</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mi>w</mi> <mrow> <mo>(</mo> <mi>L</mi> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>7</mn> <mo>)</mo> </mrow> </mrow>
wherein∑PAnd delta (P-L) are respectively calculated by a formula (2) and a formula (4); solving the nonlinear minimization problem of equation (7) using the Levenberg algorithm with an optimized uncertainty of the three-dimensional line of
<mrow> <msub> <mo>&Sigma;</mo> <mi>L</mi> </msub> <mo>=</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>J</mi> <mi>w</mi> <mi>T</mi> </msubsup> <msubsup> <mo>&Sigma;</mo> <mi>w</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <msub> <mi>J</mi> <mi>w</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow>
Wherein,
4. the feature-fused RGB-D camera motion estimation method according to claim 1, wherein said step three comprises the steps of:
for the point features, SURF descriptors of the point features are calculated, and the point features of two adjacent frames of images are matched through the similarity between the descriptors; for straight line features, a mean normalized descriptor is calculated and then matched.
5. The feature-fused RGB-D camera motion estimation method according to claim 1, wherein said step four includes the steps of:
t represents the motion conversion of two adjacent frames, and comprises T (X), RX + T, wherein R is a rotation matrix, T is a displacement vector, and a six-dimensional vector ξ is formed by taking the non-0 items of the rotation matrix and the displacement vector;
to be provided withAs a set of matched three-dimensional point sets of two adjacent frames F and F',then the three-dimensional straight line feature matching pairs of two adjacent frames are obtained; if three-dimensional point matching pairsThe corresponding real physical point is X under the local coordinate of the previous frameiThree-dimensional points, matching pairs in three-dimensional linesCorresponding to a true physical straight line of YjThen, the variable to be optimized in the camera odometer system is
The error function defining the system includes errors for point features and errors for straight line features, i.e.
<mrow> <mi>f</mi> <mrow> <mo>(</mo> <mi>&Delta;</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>f</mi> <mi>P</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>f</mi> <mi>L</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow>
Wherein the error of the point feature is
<mrow> <msub> <mi>f</mi> <mi>P</mi> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>X</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>P</mi> <mn>1</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>X</mi> <mi>n</mi> </msub> <mo>-</mo> <msub> <mi>P</mi> <mi>n</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>T</mi> <mrow> <mo>(</mo> <msub> <mi>X</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mo>-</mo> <msubsup> <mi>P</mi> <mn>1</mn> <mo>&prime;</mo> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>T</mi> <mrow> <mo>(</mo> <msub> <mi>X</mi> <mi>n</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <msubsup> <mi>P</mi> <mi>n</mi> <mo>&prime;</mo> </msubsup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
Error of straight line feature is
<mrow> <msub> <mi>f</mi> <mi>L</mi> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>&eta;</mi> <mrow> <mo>(</mo> <msub> <mi>L</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>Y</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&eta;</mi> <mrow> <mo>(</mo> <msub> <mi>L</mi> <mi>m</mi> </msub> <mo>-</mo> <msub> <mi>Y</mi> <mi>m</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&eta;</mi> <mrow> <mo>(</mo> <msubsup> <mi>L</mi> <mn>1</mn> <mo>&prime;</mo> </msubsup> <mo>-</mo> <mi>T</mi> <mo>(</mo> <msub> <mi>Y</mi> <mn>1</mn> </msub> <mo>)</mo> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&eta;</mi> <mrow> <mo>(</mo> <msubsup> <mi>L</mi> <mi>m</mi> <mo>&prime;</mo> </msubsup> <mo>-</mo> <mi>T</mi> <mo>(</mo> <msub> <mi>Y</mi> <mi>m</mi> </msub> <mo>)</mo> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
Error of a single straight line is η (L)i-Yi)＝[δ(Ai,Yi)T,δ(Bi,Yi)T]And is and
setting the target to find the variable Δ that minimizes the system error of the camera odometer system, i.e.
<mrow> <munder> <mrow> <mi>m</mi> <mi>i</mi> <mi>n</mi> </mrow> <mi>&Delta;</mi> </munder> <mi>f</mi> <msup> <mrow> <mo>(</mo> <mi>&Delta;</mi> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msubsup> <mo>&Sigma;</mo> <mi>f</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mi>f</mi> <mrow> <mo>(</mo> <mi>&Delta;</mi> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow>
Wherein, sigmaf＝diag(ΣP,ΣL) Solving the above formula (10) by using a Levenberg algorithm to obtain an initial pose of the camera motion; and constructing a pose graph by using initial pose information of camera motion, and optimizing the motion track of the camera by using the pose graph at the back end of the SLAM through a graph optimization method.
6. The method as claimed in claim 3, wherein the step 2 of discarding the points with abnormal depth values is discarding the points with null depth values.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711297500.6A CN108053445A (en) | 2017-12-08 | 2017-12-08 | The RGB-D camera motion methods of estimation of Fusion Features |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711297500.6A CN108053445A (en) | 2017-12-08 | 2017-12-08 | The RGB-D camera motion methods of estimation of Fusion Features |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN108053445A true CN108053445A (en) | 2018-05-18 |
Family
ID=62123166
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201711297500.6A Pending CN108053445A (en) | 2017-12-08 | 2017-12-08 | The RGB-D camera motion methods of estimation of Fusion Features |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108053445A (en) |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109993747A (en) * | 2019-03-22 | 2019-07-09 | 上海理工大学 | Merge the rapid image matching method of dotted line feature |
| CN110162038A (en) * | 2019-05-07 | 2019-08-23 | 杭州迦智科技有限公司 | Control method for movement, device, storage medium and processor |
| CN110517301A (en) * | 2019-07-22 | 2019-11-29 | 杭州电子科技大学 | The method of feature is effectively matched under a kind of quick camera motion |
| CN110807799A (en) * | 2019-09-29 | 2020-02-18 | 哈尔滨工程大学 | Line feature visual odometer method combining depth map inference |
| CN111047620A (en) * | 2019-11-15 | 2020-04-21 | 广东工业大学 | Unmanned aerial vehicle visual odometer method based on depth point-line characteristics |
| CN114800504A (en) * | 2022-04-26 | 2022-07-29 | 平安普惠企业管理有限公司 | Robot posture analysis method, device, equipment and storage medium |
| CN117351140A (en) * | 2023-09-15 | 2024-01-05 | 中国科学院自动化研究所 | Three-dimensional reconstruction method, device and equipment integrating panoramic camera and laser radar |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102692236A (en) * | 2012-05-16 | 2012-09-26 | 浙江大学 | Visual milemeter method based on RGB-D camera |
| CN104851094A (en) * | 2015-05-14 | 2015-08-19 | 西安电子科技大学 | Improved method of RGB-D-based SLAM algorithm |
| EP2910187A1 (en) * | 2014-02-24 | 2015-08-26 | Université de Strasbourg (Etablissement Public National à Caractère Scientifique, Culturel et Professionnel) | Automatic multimodal real-time tracking of moving instruments for image plane alignment inside a MRI scanner |
| CN105783913A (en) * | 2016-03-08 | 2016-07-20 | 中山大学 | SLAM device integrating multiple vehicle-mounted sensors and control method of device |
| CN105938619A (en) * | 2016-04-11 | 2016-09-14 | 中国矿业大学 | Visual odometer realization method based on fusion of RGB and depth information |
| CN106127739A (en) * | 2016-06-16 | 2016-11-16 | 华东交通大学 | A kind of RGB D SLAM method of combination monocular vision |
| CN106780592A (en) * | 2016-06-30 | 2017-05-31 | 华南理工大学 | Kinect depth reconstruction algorithms based on camera motion and image light and shade |
-
2017
- 2017-12-08 CN CN201711297500.6A patent/CN108053445A/en active Pending
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102692236A (en) * | 2012-05-16 | 2012-09-26 | 浙江大学 | Visual milemeter method based on RGB-D camera |
| EP2910187A1 (en) * | 2014-02-24 | 2015-08-26 | Université de Strasbourg (Etablissement Public National à Caractère Scientifique, Culturel et Professionnel) | Automatic multimodal real-time tracking of moving instruments for image plane alignment inside a MRI scanner |
| CN104851094A (en) * | 2015-05-14 | 2015-08-19 | 西安电子科技大学 | Improved method of RGB-D-based SLAM algorithm |
| CN105783913A (en) * | 2016-03-08 | 2016-07-20 | 中山大学 | SLAM device integrating multiple vehicle-mounted sensors and control method of device |
| CN105938619A (en) * | 2016-04-11 | 2016-09-14 | 中国矿业大学 | Visual odometer realization method based on fusion of RGB and depth information |
| CN106127739A (en) * | 2016-06-16 | 2016-11-16 | 华东交通大学 | A kind of RGB D SLAM method of combination monocular vision |
| CN106780592A (en) * | 2016-06-30 | 2017-05-31 | 华南理工大学 | Kinect depth reconstruction algorithms based on camera motion and image light and shade |
Non-Patent Citations (2)
| Title |
|---|
| YAN LU 等: "Robust RGB-D Odometry Using Point and Line Features", 《2015 IEEE INTERNATIONAL CONFERENCE ON COMPUTER VISION》 * |
| 王亚龙 等: "基于Kinect的三维视觉里程计的设计", 《计算机应用》 * |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109993747A (en) * | 2019-03-22 | 2019-07-09 | 上海理工大学 | Merge the rapid image matching method of dotted line feature |
| CN110162038A (en) * | 2019-05-07 | 2019-08-23 | 杭州迦智科技有限公司 | Control method for movement, device, storage medium and processor |
| CN110517301A (en) * | 2019-07-22 | 2019-11-29 | 杭州电子科技大学 | The method of feature is effectively matched under a kind of quick camera motion |
| CN110517301B (en) * | 2019-07-22 | 2022-04-01 | 杭州电子科技大学 | Method for effectively matching features under rapid camera motion |
| CN110807799A (en) * | 2019-09-29 | 2020-02-18 | 哈尔滨工程大学 | Line feature visual odometer method combining depth map inference |
| CN110807799B (en) * | 2019-09-29 | 2023-05-30 | 哈尔滨工程大学 | Line feature visual odometer method combined with depth map inference |
| CN111047620A (en) * | 2019-11-15 | 2020-04-21 | 广东工业大学 | Unmanned aerial vehicle visual odometer method based on depth point-line characteristics |
| CN114800504A (en) * | 2022-04-26 | 2022-07-29 | 平安普惠企业管理有限公司 | Robot posture analysis method, device, equipment and storage medium |
| CN117351140A (en) * | 2023-09-15 | 2024-01-05 | 中国科学院自动化研究所 | Three-dimensional reconstruction method, device and equipment integrating panoramic camera and laser radar |
| CN117351140B (en) * | 2023-09-15 | 2024-04-05 | 中国科学院自动化研究所 | Three-dimensional reconstruction method, device and equipment integrating panoramic camera and laser radar |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Fan et al. | Road surface 3D reconstruction based on dense subpixel disparity map estimation | |
| CN108053445A (en) | The RGB-D camera motion methods of estimation of Fusion Features | |
| WO2022188094A1 (en) | Point cloud matching method and apparatus, navigation method and device, positioning method, and laser radar | |
| CN108229416B (en) | Robot SLAM method based on semantic segmentation technology | |
| CN107358629B (en) | Indoor mapping and positioning method based on target identification | |
| CN113223045B (en) | Vision and IMU sensor fusion positioning system based on dynamic object semantic segmentation | |
| CN111862673A (en) | Parking lot vehicle self-positioning and map construction method based on top view | |
| Reina et al. | 3D traversability awareness for rough terrain mobile robots | |
| CN104281148A (en) | Mobile robot autonomous navigation method based on binocular stereoscopic vision | |
| Chen et al. | A stereo visual-inertial SLAM approach for indoor mobile robots in unknown environments without occlusions | |
| CN111998862A (en) | Dense binocular SLAM method based on BNN | |
| Guan et al. | Minimal solvers for relative pose estimation of multi-camera systems using affine correspondences | |
| CN109544632B (en) | Semantic SLAM object association method based on hierarchical topic model | |
| Beauvisage et al. | Robust multispectral visual-inertial navigation with visual odometry failure recovery | |
| CN112731503A (en) | Pose estimation method and system based on front-end tight coupling | |
| Wong et al. | Monocular localization within sparse voxel maps | |
| Aggarwal | Machine vision based SelfPosition estimation of mobile robots | |
| CN113744301B (en) | Motion trail estimation method and device for mobile robot and storage medium | |
| Kim et al. | Image-Based ICP algorithm for visual odometry using a RGB-D sensor in a dynamic environment | |
| Song et al. | MF-LIO: integrating multi-feature LiDAR inertial odometry with FPFH loop closure in SLAM | |
| Sun | Dance training movement depth information recognition based on artificial intelligence | |
| Ren | An improved binocular LSD_SLAM method for object localization | |
| Zhi et al. | A Dynamic Visual-Inertial-Wheel Odometry With Semantic Constraints And Denoised IMU-Odometer Prior For Autonomous Driving | |
| Shilin et al. | Application of a Depth Camera for Constructing Complex Three-Dimensional Models in Multiple Scanning Complexes | |
| Tamura et al. | Visual odometry with effective feature sampling for untextured outdoor environment |
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 | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20180518 |