Method of three dimensional positioning using feature matching

[0001] This is a regular application of a provisional application, provisional application No. 60/492,545, filed Aug. 4, 2003.

[0002] FEDERAL RESEARCH STATEMENT: The present invention is made with U.S. Government support under contract number W15QKN-04-C-1003 awarded by the Department of the Army. The Government has certain rights in the invention.

[0003] The present invention relates generally to machine vision systems, and more particularly to object positioning, which employs electro-optic (EO) image sensors enhanced with integrated laser ranger, global positioning system/inertial measurement unit, and integrates this data to get reliable and real time object position.

[0004] There are two difficult problems for machine vision systems. One is image processing speed, another is the reliability, which affect the application of electro-optic image sensors (e.g. stereo cameras) to robotics, autonomous landing and material handling.

[0005] The match filter takes long time to match different orientation and size templates to detect certain object. In the present invention, the electro-optic image sensor imaging system derives the electro-optic image sensors' attitude and orientation data from a global positioning system/inertial measurement unit integrated navigation system for the template rotation. The electro-optic image sensor image system derives the object range data from a laser ranger for the template enlarging/shrinking.

[0006] It is a main objective of the present invention to provide electro-optic image sensor positioning using feature matching thereof, in which three dimensional object positions can be calculated and determined.

[0007] Another objective of the present invention is to provide a universal object positioning and data integrating method and system thereof, in which the electro-optic image sensor imaging system derives the electro-optic image sensors' attitude and orientation data from a global positioning system/inertial measurement unit integrated navigation system for the template rotation.

[0008] Another objective of the present invention is to provide a universal object positioning and data integrating method and system thereof, in which the electro-optic image sensor image system derives the object range data from a laser ranger for the template enlarging/shrinking.

[0009] Another objective of the present invention is to provide a universal fiducial feature detection method and system thereof, in which the autonomous object positions can be calculated and determined by matching fiducial features in both images.

[0010] Another objective of the present invention is to provide a universal corner detection method and system thereof, in which the autonomous object positions can be calculated and determined by matching corners in both images.

[0011] Another objective of the present invention is to provide an object identification method and system thereof, in which the detected three dimensional corners and fiducial features are grouped for object identification.

[0012] Another objective of the present invention is to provide an object identification method and system thereof, in which the grouped corner and fiducial features are combined with line detection and circle detection for complex object detection.

[0013] The key to electro-optic sensors image processing is to determine which point in one image corresponds to a given point in another image. There are many methods that deal with this problem, such as the correlation method, gray level matching and graph cut. These methods process all of the image pixels for both electro-optic image sensors. Some of the methods need iteration until convergence occurs. Pixel matching is time consuming and unreliable. Actually only the feature points can be used for positioning. Points, corners, lines, circles and polygons are four types of image features. Corners and fiducial features are selected for processing.

[0014] Image segmentation is an essential procedure to extract features from images. For the segmentation, the high pass filter is used to segment an image. To improve the results of segmentation, a smoothing preprocessing operation is preferable. In order to preserve edge information in a smoothing procedure, the low pass filter is used at the preprocessing stage.

[0015] Because the fiducial features can be consistently detected in the images, the same features can be found in both images correctly by processing electro-optic sensor images and matching features on both images. The disparities (i.e., range) to the fiducial features can be easily computed.

[0016] A corner is another kind of feature, which exists in most kinds of objects. Corner detection utilizes the convolution between the original image and a corner mask, so it can be implemented in real time easily. The same corners can be found in both images, correctly, by processing the electro-optic sensor images and matching corners in both images. The disparities (i.e., range) to the corners can then be easily computed.

[0017] The present invention can substantially solve the problems encountered in machine vision system integration by using state-of-the-art inertial sensor, global positioning system technology, laser ranger unit enhanced with fiducial feature matching and corner matching technologies. The present invention is to make machine vision a practical application by enhancing real time and reliability.

[0025] Generally, IMU/GPS integration can output the position, attitude and azimuth of the vehicle itself. A laser ranger measures the distance between the object and vehicle. Electro-optic image sensors derive the 3D environment in the field of view. The traditional electro-optic sensor image processing is time consuming and unreliable.

[0026] Referring to FIG. 1, the electro-optic sensor image processing comprises a preprocessing module 1, a segmentation module 2, a detection module 3, a recognition module 4, a 3D-positioning module 5, a tracking module 6, EO sensors 7, an AHRS/INS/GPS Integration module 8, a GPS receiver 9, a MEMS IMU 10, and a laser ranger 11.

[0027] Referring to FIG. 2, the preprocessing module 1 comprises a Median Filter module 11, a Histogram Equalization module 12 and an Inverse Image module 13.

[0028] Referring to FIG. 3, the segmentation module 2 comprises a Threshold Black/White module 21, a Suppress Black module 22, a Suppress White module 23, and a Sobel Filter module 24.

[0029] Edge detection is necessary to detect meaningful discontinuities in gray level. Line detection and circle detection also uses edge detection for segmentation. Hence, a Sobel filter is employed for edge detection. The Sobel filter masks are defined as follows,

[0030] $\left[\begin{array}{ccc}-1& -2& -1\\ 0& 0& 0\\ 1& 2& 1\end{array}\right]\left[\begin{array}{ccc}-1& 0& 1\\ -2& 0& 2\\ -1& 0& 1\end{array}\right]$

[0031] Referring to FIG. 4, the detection module 3 comprises a Line Detection module 31, a Circle Detection module 32, a Corner Detection module 33, a Gabor Filter module 34, and an Eigenvector Projection module 35.

[0032] Line detection and circle detection are defined in this paragraph. The equation of a straight line is x cos θ+y sin θ=ρ. In the ρθ plane the straight lines are sinusoidal curves. Binarized points (i, j) in the ρθ plane are used as locations of associated (ρ_i,θ_j) pairs that satisfy the equation of the straight line. Similarly, for a circle the equation utilized is (x−c₁)²+(y−c₂)²=c₃². The Hough transform can be generalized to apply to any equation of the form g(v,c)=0, where v represents the vector of coordinates and c is the vector of coefficients. The difference from the 2D parameters case is that the 2D cells and points (i,j) are replaced by the higher order cells and points (i,j,k).

[0033] The Gabor filter is used to detect the fiducial features. Matching the fiducial features in both electro-optic sensors' images results in calculation of the 3D positions of the fiducial features. The Gabor filter output is constructed as follows:

[0034] $C\left(x,y\right)=\underset{\theta }{\max }I\left(x,y\right)\otimes {\Phi }_j\left(x,y,\theta \right)-s_j/s_{k}I\left(x,y\right)\otimes {\Phi }_k\left(x,y,\theta \right)$
where k=11, j=10, s_j=2^j/2 (scale factor), I(x,y) is the original image
Φ_j(x,y,θ)=Φ(s_jx,s_jy,θ)
Φ(x,y,θ)=e^{−(x′2+y′2)+iπx′}
x′=x cos θ+y sin θ
y′=−x sin θ+y cos θ
θ=0, 90, 180 and 270 degrees (Orientation)

[0035] A variety of methods of corner detection can be used to detect the corners of the objects. Matching the corners in both electro-optic sensors' images results in calculation of the 3D positions of the corners.

[0036] Referring to FIG. 5, the recognition module 4 comprises a Matched Filter module 41, a Graph Matching module 42, a Corner Classifier module 43, and a Neural Network module 44.

[0037] Template correlation needs the reference object template and an image patch selected from the new image frame. Denote the two two-dimensional scenes of N×M pixels by I(k, l) and J(k, l) where k and l stand for the row index and column index, respectively. A direct method would compute the cross correlation function between I(k, l)and J(k, l) defined as

[0038] $R\left(n,m\right)=\frac{1}{\mathrm{MN}}\sum _{k=1}^N\sum _{l=1}^{M}I\left(k,l\right)J\left(k+n,l+m\right)$

[0039] where n and m are the lag length in the row and column directions, respectively. However, the evaluation of the correlation is computationally intensive and normally not used in practice. A common way to calculate the cross correlation other than direct computation is to use the fast Fourier transformation (FFT).

[0040] In fact, a variety of methods can be used to speed up the computation of the correlation function or to reduce the amount of memory required. Division of the two-dimensional array into subarrays where only partial convolutions will be computed allows for tradeoffs between speed, memory requirements, and total lag length. In addition, since an FFT algorithm can accept complex inputs, the processing of two real series can be made in parallel. Moreover, the application of number theoretic results in the FFT transformation can take into account quantization of numbers and the finite precision of digital machines in the digital transforms. Finally, the use of specific hardware such as pipelined processing is now practical to further increase the real-time computation speed.

[0041] Referring to FIG. 6, the tracking module 5 comprises a Peak Tracking module 51, a Centroiding Tracking module 52 and a Relative Position Tracking module 54.

[0042] Referring to FIG. 7, two electro-optic image sensors are fixed on the vehicle with their optical axes parallel. The baseline b is perpendicular to the optical axes. The vehicle body frame can be established as shown in FIG. 7. Let the baseline be the x-axis, z-axis parallel to optical axis and origin at the center of baseline, and the image coordinates in left and right images be (x_l′, y_l′) and (x_r′, y_r′), respectively. Then

[0043] $\frac{x_l^{\prime }}{f}=\frac{x+b/2}{z},\frac{x_r^{\prime }}{f}=\frac{x-b/2}{z}\mathrm{and}\frac{y_l^{\prime }}{f}=\frac{y_r^{\prime }}{f}=\frac{y}{z}$

[0044] where f is the focal length.

[0045] from the above equations, we get

[0046] $\begin{array}{c}x=b\frac{\left(x_l^{\prime }+x_r^{\prime }\right)/2}{x_l^{\prime }-x_r^{\prime }},\\ y=b\frac{\left(y_l^{\prime }+y_r^{\prime }\right)/2}{x_l^{\prime }-x_r^{\prime }},\mathrm{and}\\ z=b\frac{f}{x_l^{\prime }-x_r^{\prime }}.\end{array}$

[0047] According to the optical principle, if pixel resolution r_p is known, we have
x_l′=f*tan(r_px_pl), y_l′=f*tan(r_py_pl)
x_r′=f*tan(r_px_pr), y_r′=f*tan(r_py_pr)

[0048] where (x_pl, y_pl) and (x_pr, y_pr) are pixel coordinates in the left and right images, respectively. Hence, the target position with respect to the vehicle frame can be calculated.

[0049] Referring to FIGS. 1-7, the method of three dimensional positioning according to the preferred embodiment of the present invention is illustrated, which comprises the steps as follows:

[0050] (1) Receive images from the EO sensors and send them to the preprocessing module 1.

[0051] (2) Perform Median Filtering to suppress noise in the Median Filter module 11 and Histogram Equalization to enhance the images in the Histogram Equalization module 12. If the object image library is black, invert the image in the Inverse Image module 13.

[0052] (3) Receive preprocessed images from the preprocessing module 1 and perform Threshold Black/White in the Threshold/White module 21, Suppress Black in the Suppress Black module 22, Suppress White in the Suppress White module 23, and edge detection in the Sobel Filter module 24.

[0053] (4) Receive segmented images from the segmentation module 2 and perform Line Detection in the Line Detection Module 31, Circle Detection in the Circle Detection module 32 and Eigenvector Projection in the Eigenvector Projection module 35.

[0054] (5) Receive the preprocessed images from the preprocessing module 1 and perform Corner Detection in the Corner Detection module 33, fiducial feature detection in the Gabor Filter module 34. Send detected corners and fiducial features to the 3D Positioning module 6 and the Recognition module 4.

[0055] (6) Receive the detected corners from the Corner Detection module 33, match the corners in the two images to get the disparities, and calculate 3D positions for each corner pair in the 3D Positioning module 6.

[0056] (7) Receive the detected fiducial features from the Gabor Filter module 34, match the corners in the two images to get the disparities, and calculate 3D positions for each corner pair in the 3D Positioning module 6.

[0057] (8) Receive detected lines, circles, corners and fiducial features from the Detection module 3, get the detected corners and fiducial 3D positions from the 3D Positioning module 6, group them in the Graph Matching module 42, Corner Classifier module 43 and Neural network module 44, to identify certain object.

[0058] (9) Receive the recognized certain object in the Relative Position Tracking module 53, wherein the recognized certain object includes calculated 3D corners and fiducial features, to get the 3D target position.

[0059] Referring to FIGS. 1-7, an alternative method of three dimensional positioning according to the preferred embodiment of the present invention is illustrated, which comprises the steps as follows:

[0060] (1) Receive the images from the EO sensors and send them to the preprocessing module 1.

[0061] (2) Perform Median Filtering to suppress noise in the Median Filter module 11 and Histogram Equalization to enhance the images in the Histogram Equalization module 12. If the object image library is black, invert the image in the Inverse Image module 13.

[0062] (3) Receive preprocessed images from the preprocessing module 1 and perform Threshold Black/White in the Threshold/White module 21, Suppress Black in the Suppress Black module 22, Suppress White in the Suppress White module 23, and edge detection in the Sobel Filter module 24.

[0063] (4) Receive segmented images from the segmentation module 2 and perform Line Detection in the Line Detection Module 31, Circle Detection in the Circle Detection module 32 and Eigenvector Projection in the Eigenvector Projection module 35.

[0064] (5) Receive preprocessed images from the preprocessing module 1 and perform Corner Detection in the Corner Detection module 33, fiducial feature detection in the Gabor Filter module 34. Send detected corners and fiducial features to the 3D Positioning module 6 and the Recognition module 4.

[0065] (6) Receive the detected corners from the Corner Detection module 433, match the corners in the two images to get the disparities, and calculate 3D positions for each corner pair in the 3D Positioning module 46.

[0066] (7) Receive the detected fiducial features from the Gabor Filter module 34, match the corners in the two images to get the disparities, and calculate 3D positions for each corner pair in the 3D Positioning module 6.

[0067] (8) Receive GPS measurements, including position, velocity and time from the global positioning system 9, and pass them to the AHRS/INS/GPS integration module 8.

[0068] (9) Receive inertial measurements including body angular rates and specific forces, from the inertial measurement unit 10, and send them to the AHRS/INS/GPS integration module 8 which is a signal-processing module.

[0069] (10) Perform inertial navigation system (INS) processing in the AHRS/INS/GPS integration module 8.

[0070] (11) Receive the laser ranger measurement from a laser ranger 11′ and send it to the recognition module 4.

[0071] (12) Receive the preprocessed images from the preprocessing module 1, match the processed target template and output to the Peak Tracking module 51 or Centroiding Tracking module in the Tracking module 52.

[0072] (13) Receive detected lines, circles, corners and fiducial features from the Detection module 3, get the detected corner and fiducial 3D positions from the 3D Positioning module 6, group them in the Graph Matching module 42, Corner Classifier module 43 and Neural network module 44, to identify the certain object.

[0073] (14) Relative Position Tracking module 53 receives the recognized certain object, which comprises calculated 3D corners and fiducial features, to get the 3D target position.

[0074] According to the preferred embodiment of the present invention, Step (12) further comprises of the following steps (as shown in FIG. 1):

[0075] (12-1) Retrieve the target knowledge database to get the target template, receive the attitude and azimuth from the AHRS/INS/GPS Integration module 8, and rotate the target template in the Matched Filter module 41.

[0076] (12-2) Receive the laser range from the Laser Ranger module 11, and shrink or enlarge the processed images from step (1) in the Matched Filter module 41.

[0077] (12-3) Do the Match Filter in the Matched Filtering module 41.

[0078] The present invention employs electro-optic image sensors integrated global positioning system/inertial measurement unit and laser ranger, to provide reliable and real time object 3D position. These data can be used by an autonomous vehicle or a robot controller. The advantages of the present invention include:

[0079] (1) The electro-optic image sensors' measures the feature and corner 3D positions. The 3D positions can be grouped with detected lines and circles for the recognition of certain objects, such as fork holes, pallets, etc.

[0080] (2) The IMU/GPS integration system provides the vehicle's attitude and azimuth so as to rotate the target library in the sensor/target knowledge database in order to match the electro-optic sensors' images. This dramatically reduces the storage volume and matching time. It is not necessary to store different kinds of objects in orientation in the sensor/target knowledge database and match the object at different orientations.

[0081] (3) The laser ranger measures the distance between the object and vehicle. This reliable distance can be used to calibrate the 3D electro-optic sensors' position. It can also be used to shrink and enlarge the target library in the sensor/target knowledge database in order to match the electro-optic sensors' images. This dramatically reduces the storage volume and matching time.