Patent Grant
8199977
This application is related to the following co-pending United States patent applications, all of which are hereby incorporated herein by reference:
U.S. patent application Ser. No. 12/436,224 entitled “SYSTEMS AND METHODS FOR EXTRACTING PLANAR FEATURES, MATCHING THE PLANAR FEATURES, AND ESTIMATING MOTION FROM THE PLANAR FEATURES” filed on May 6, 2009 and which is referred to herein as the '224 application; and
U.S. patent application Ser. No. 12/234,331 entitled “SYSTEM AND METHOD OF EXTRACTING PLANE FEATURES,” filed on Sep. 19, 2008 and referred to herein as the '331 application.
U.S. patent application Ser. No. 12/644,559 entitled “SYSTEMS AND METHODS FOR MATCHING SCENES USING MUTUAL RELATIONS BETWEEN FEATURES,” filed on Dec. 22, 2009 and referred to herein as the '559 application.
Typical vision-based navigation systems identify simple uniquely identifiable objects (commonly referred to as features) in a two-dimensional (2-D) image (typically an intensity image acquired by electro-optical cameras). These features and their position within the image are used for further processing, such as more complex object detection or motion estimation.
In one embodiment, a method of extracting a feature from a point cloud is provided. The method comprises receiving a three-dimensional (3-D) point cloud representing objects in a scene, the 3-D point cloud containing a plurality of data points; generating a plurality of hypothetical features based on data points in the 3-D point cloud, wherein the data points corresponding to each hypothetical feature are inlier data points for the respective hypothetical feature; and selecting the hypothetical feature having the most inlier data points as representative of an object in the scene.
Understanding that the drawings depict only exemplary embodiments and are not therefore to be considered limiting in scope, the exemplary embodiments will be described with additional specificity and detail through the use of the accompanying drawings, in which:
In accordance with common practice, the various described features are not drawn to scale but are drawn to emphasize specific features relevant to the exemplary embodiments.
In the following detailed description, reference is made to the accompanying drawings that form a part hereof, and in which is shown by way of illustration specific illustrative embodiments. However, it is to be understood that other embodiments may be utilized and that logical, mechanical, and electrical changes may be made. Furthermore, the method presented in the drawing figures and the specification is not to be construed as limiting the order in which the individual steps may be performed. The following detailed description is, therefore, not to be taken in a limiting sense.
In this exemplary embodiment, the data points collected by sensor 102 comprise a three dimensional (3D) point cloud. In one embodiment, sensor 102 is a 3D scanning or flash LiDAR camera such as the Swissranger SR-3100 manufactured by Mesa Imaging AG. In another embodiment, sensor 102 is a 3D scanning rotating LiDAR, such as LiDAR manufactured by the Velodyne corporation. In yet another embodiment, sensor 102 is a radar sensor (e.g. a millimeter wave radar or weather radar). In yet another embodiment, sensor 102 comprises a combination of radar and an electro-optical camera. In other embodiments, other combinations of sensors are used. In other embodiments, the 3D point-cloud is generated from data gathered over time or along a motion trajectory using sensors such as, but not limited to, sonar sensors, laser or radar altimeters, or sensors used for surveying methods, bathymetry, radar topography, structure from motion methods, or interferometry.
Processing unit 104 uses the 3D data points provided by sensor 102 to identify planar features in the scene. A planar feature has the appearance of a geometric plane and is defined by a set of points. In other words, if all the points in a set x satisfy the geometric equation of a plane within some small margin or threshold t, as shown in equation 1 below, then the set of points x defines a planar feature.
|n′x−d|≦t, Eq. 1
where n is the normal vector of the planar feature and d is the orthogonal distance from the planar feature to the origin.
The normal vector n and the orthogonal distance d can be estimated from the set of points. The points in the set of points which define a planar feature are also referred to herein as inliers or inlier data points. Similarly, the points which do not meet the above criteria for a given planar feature are referred to as outliers. The planar features typically correspond to real world planes on objects within a scene. For example, planar features may correspond to a top of a table, a side of a box, or a wall of a room, etc.
It should also be noted that, in some implementations, sensor(s) 102 provide range data in spherical coordinates (i.e. a horizontal angle, a vertical angle, and a distance). The spherical coordinates are converted to Cartesian coordinates, either by the sensor 102 itself or by processing unit 104. In an alternative embodiment, the calculations that follow are adapted to spherical coordinates. In other embodiments, the approach described below is generalized to operate on data using other mathematical representations of the input data. Using other mathematical representations of the input data enables extraction of features other than Cartesian planes, such as various two-dimensional manifold shapes. Thus, the description of the Cartesian coordinate space is provided by way of example and is not to be taken as limiting the scope of the present application. It should also be noted that the calculations presented herein can be modified to extract other geometrical two-dimensional manifolds which can be expressed, for example, by the mathematical equation ƒ(x, θ)≦t, where x denotes points, θ denotes a set of geometrical parameters that can be estimated, and ƒ is a function. In the case of planar features described herein, the equation is expressed as ƒ(x, θ)=|n′x−d|, and θ={n, d}. For an exemplary case of spherical features, the equation can be expressed as ƒ(x, θ)=|(x−a)′(x−a)−r2| and θ={a, r}. Thus, the mathematical equation ƒ(x, θ)≦t, is the generalized equation for determining the point-to-feature distance of each point, where each point whose point-to-feature distance is less than or equal to the threshold is identified as an inlier data point of that feature. As used herein the term “feature” is used to refer to planes and other two-dimensional manifolds such as spherical features. It is to be understood that the description below regarding planar features can also be adapted to other features, such as spherical features.
Furthermore, in this exemplary embodiment, processing unit 104 estimates motion by matching extracted planar features from the current scene to the corresponding planar features in a previous scene stored in the memory 106. For example, in some embodiments, processing unit 104 uses techniques such as, but not limited to, Scale-invariant feature transform (SIFT), Speeded Up Robust Features (SURF), or other matching techniques as known to one of skill in the art. In addition, in some embodiments, the techniques described in the '559 application are used to match extracted planar features. Hence, in this embodiment, processing unit 104 utilizes the planar features for visual odometry. However, in other embodiments, processing unit 104 is operable to use the planar features for other navigation tasks, such as, but not limited to, integration of the observed scenes with map data to minimize misalignment of map data to the real world, and automatic map construction based on the observed scenes.
Additionally, in this embodiment, system 100 includes a display unit 110 and one or more actuators 108. However, it is to be understood that, in other embodiments, one or both of display unit 110 and actuators 108 are omitted. Display unit 110 is operable to display navigation data to a user. For example, if the planar features are used to estimate motion, the calculated position and orientation of system 100 is displayed to the user. Alternatively, if the planar features are used for map generation, the generated map is displayed to the user. Suitable display units include, but are not limited to, various CRT, active and passive matrix LCD, and plasma display units.
In some embodiments, processing unit 104 uses the estimated motion calculation to determine the necessary actions to take in order to reach a programmed destination and/or avoid obstacles. In some such embodiments, processing unit 104 generates control signals which are sent to actuators 108 to control the movement of a vehicle in which system 100 is located. For example, processing unit 104 can control the motion of an unmanned vehicle based on control signals transmitted to movement actuators (such as the brakes, accelerator, etc.) in the unmanned vehicle. In other embodiments, the estimated motion calculation is used for fusion with data from other sensors, such as alternate navigation sensor 105, in order to achieve improved reliability and/or accuracy. For example, alternate navigation sensor 105 can be implemented as, but is not limited to, an inertial measurement unit (IMU), inertial navigation system (INS), attitude and heading reference system (AHRS), or other type of navigation system.
To identify and extract planar features from the 3D data points received from sensor 102, processing unit 104 executes code 112 stored in the memory 106. In particular, code 112 causes the processing unit 104 to execute one or more of the methods described in more detail below with respect to
These instructions are typically stored on any appropriate computer readable medium used for storage of computer readable instructions or data structures. The computer readable medium can be implemented as any available media that can be accessed by a general purpose or special purpose computer or processor, or any programmable logic device. Suitable processor-readable media may include storage or memory media such as magnetic or optical media. For example, storage or memory media may include conventional hard disks, Compact Disk-Read Only Memory (CD-ROM), volatile or non-volatile media such as Random Access Memory (RAM) (including, but not limited to, Synchronous Dynamic Random Access Memory (SDRAM), Double Data Rate (DDR) RAM, RAMBUS Dynamic RAM (RDRAM), Static RAM (SRAM), etc.), Read Only Memory (ROM), Electrically Erasable Programmable ROM (EEPROM), and flash memory, etc. Suitable processor-readable media may also include transmission media such as electrical, electromagnetic, or digital signals, conveyed via a communication medium such as a network and/or a wireless link. In addition, in some embodiments, the processing unit 104 is implemented as a Field-Programmable Gate Array (FPGA) containing an equivalent representation of the computer readable instructions.
At block 206, a plurality of hypothetical planes is generated based on the points in the 3-D point cloud. When the 3-D point cloud is divided into cells, a plurality of hypothetical planes is generated for each cell based on the points within the respective cell. Exemplary methods of generating each of the plurality of hypothetical planes are described in more detail in
At block 208, one of the plurality of hypothetical planes is selected as representative of a plane of an object in the scene. In particular, the selected hypothetical plane is the plane which provides a value of a quality function that is closer to a desired value than the non-selected hypothetical planes. A quality function, as used herein, is defined as a function whose value assess the goodness (or the quality) of a candidate or hypothetical feature. It should be noted that an explicit comparison with a desired value is not required in some embodiments.
For example, in the embodiment of
It is to be understood that other quality functions can be used in other embodiments. Other exemplary quality functions include, but are not limited to, a function of the variance of inliers-to-plane distance and a function of the plane point density. For example, the function of the plane point density can be expressed as −|iv−plane point density|, where iv is a desired value and the plane point density is the number of inliers divided by the estimated plane size. In such embodiments, an explicit comparison is made through the quality function. Thus, variations of different quality functions can be implemented in different embodiments.
Having a hypothetical plane with parameters n (the normal vector) and d (the orthogonal distance), the number of inliers is calculated as the number of points x within the cell (or within the entire point cloud, if no cell splitting was performed), that satisfy equation 1 described above, for a given (e.g. user-specified) margin or threshold t. In one embodiment, the margin is selected to be t=0.1 m. The equation 1 described above determines the point-to-plane distance for each point. If the point-to-plane distance for a given point is less than the threshold value, 0.1 m in this example, then that point is included in the set of inlier data points x.
At block 210, the selected plane is optionally refined to improve the estimate of the planar feature. Exemplary methods of refining the selected plane are described in more detail with respect to
At block 212, the parametric description of the extracted plane is optionally computed. For example, for further processing, such as matching planar features between scenes, it might not be feasible to use the inliers of the plane directly due to computational and memory demands. Hence, in some implementations, the selected plane is described by the normal vector and orthogonal distance, in addition to other parameters such as the mean point (also referred to as centroid), to reduce the data size. For the mean point, all the points on the plane are used to compute the arithmetic mean. Also, since the inliers are a set of points, a 3-D covariance of those points and additional parameters such as the number of points can also be used as parameters to describe the plane. The parametric description is then used to determine if a match is found in another scene. In this way, the computational and memory demands are reduced compared to using the full set of inliers directly.
At block 214, if the received point cloud is divided into cells, selected planes from neighboring cells are optionally merged together. In particular, if the respective normal vectors and orthogonal distances to the origin of the selected planes are within a predetermined or adaptively determined merge threshold of each other, the selected planes are determined to belong to the same plane and are merged together. In other embodiments, plane parameters other than the normal vector and the orthogonal distance from origin are used, including but not limited to the laser return intensity, the variation in the laser return intensity, the planes' centroid-to-plane distances, polarization, principal moments, and/or others.
At block 216, a parametric description of the merged plane is optionally recalculated after merging. In some embodiments, the parametric description is recalculated based solely on the parametric description of the respective planes before they were merged together, thus reducing the computational and memory demands in comparison to using the full set of inliers directly. In some embodiments, the parametric description is recalculated immediately when any planes are merged together.
It is to be understood that blocks 206 to 212 can be repeated to select a plurality of planes. For example, in some embodiments, blocks 206 to 212 are repeated until all the points in the point cloud or respective cell are determined to be part of a plane or until a maximum defined number of iterations have been performed. In some embodiments, each iteration is performed on the set of data points reduced by the number of data points already identified as inliers of a plane in previous iterations. In other embodiments, all the input point are used in the successive iterations and additional logic is used to ensure the solutions' uniqueness, such as, but not limited to, filtration of the hypotheses generated in the block 206 to be non-repeating. In addition, whereas in this example, only one plane is selected at block 208, in other implementations multiple hypothetical planes may be selected. For example, each hypothetical plane having more than a threshold number of inliers is selected in some embodiments. Similarly, in some embodiments, each hypothetical plane having less than a threshold number of inliers is discarded. In some embodiments, the threshold varies with plane parameters. For example, in some embodiments, a smaller threshold is used for a plane having a centroid farther from the coordinates' origin than for a plane closer to the coordinates' origin.
In method 300 above, a plane has to cover a substantial part of the cell to get a reasonably high probability of the three randomly selected points belonging to the same plane. However, the method 350 only assumes that a plane is at least a partially continuous structure and if a randomly select point belongs to the plane, its small neighborhood belongs there, too. The method 350 starts with block 352, when a random point within a cell is selected. At block 354, a small neighborhood around the selected point is taken. In one embodiment, for Velodyne LiDAR, 6 adjacent lasers and 1.2° azimuth span for the neighborhood is used. In other embodiments, different neighborhood sizes are used.
At block 356, it is determined whether there are enough points within the neighborhood. In particular, in this embodiment, it is determined if the number of points is greater or equal to a given threshold. The given threshold is set to at least 3, since 3 points are needed to define a plane. In one embodiment, half of the expected points within the neighborhood are used as the threshold. The number of expected points can be determined from sensor characteristics. If the number of points is below the threshold the hypothesis is considered to be invalid at block 362 and another hypothesis may be generated.
If the number of points is greater than or equal to the threshold, method 350 continues at block 358. At block 358, the normal vector and the orthogonal distance of the hypothetical plane is estimated from all the points within the selected neighborhood. As described above, any commonly known mathematical estimator, such as a least squares estimator, can be used to estimate the normal vector and the orthogonal distance of the hypothetical plane.
At block 360, all the points within the selected neighborhood are optionally examined to determine whether they form a plane-like structure. In particular, the normal vector n and the orthogonal distance d, estimated at block 358, are used to determine how many points x within the neighborhood of points used to estimate n and d satisfy the plane constraint |n′x−d|≦t. In one embodiment, the threshold is set to t=0.05 m. In one embodiment, 80% of points x within the neighborhood need to satisfy the constraint, otherwise the hypothesis is declared to be invalid.
At block 404, a new set of inliers is determined as all points x within the cell that satisfy equation 1 above. At block 406, it is determined if the number of inliers increased. For example, in some embodiments, method 400 is repeated until no new inliers are found. If the number of inliers increased, method 400 continues at block 408 where a check is performed to determine whether a maximum number of iterations has been reached. In one embodiment, only a single iteration through the cycle is used. In other embodiments, higher numbers of iterations are used. If the maximum number of iterations has not been reached, the method continues at block 402. If the maximum number of iterations has been reached, method 400 ends at block 410. In yet another embodiment, a different metric is calculated and the refinement is repeated until there is no improvement of the respective metric.
When L iterations of method 550 have been performed, the method 500 continues by selecting from L sets of inliers the set with highest number of inliers, at block 504. In one embodiment, when multiple sets have the same highest number of inliers, the first set with the highest number of inliers is selected. In other embodiments, different selection criterions are employed.
At block 506, the normal vector and orthogonal distance are again estimated from the selected highest-number-of-inliers set. After updating the normal vector and the orthogonal distance, the final inliers are determined as a set of points x within the cell, which satisfy the equation 1 described above, at block 508, where the method 500 ends.
Although specific embodiments have been illustrated and described herein, it will be appreciated by those of ordinary skill in the art that any arrangement, which is calculated to achieve the same purpose, may be substituted for the specific embodiments shown. Therefore, it is manifestly intended that this invention be limited only by the claims and the equivalents thereof.
| Number | Name | Date | Kind |
|---|---|---|---|
| 4891630 | Friedman et al. | Jan 1990 | A |
| 5383013 | Cox | Jan 1995 | A |
| 5870490 | Takahashi et al. | Feb 1999 | A |
| 5978504 | Leger | Nov 1999 | A |
| 5988862 | Kacyra et al. | Nov 1999 | A |
| 6307959 | Mandelbaum | Oct 2001 | B1 |
| 6704621 | Stein et al. | Mar 2004 | B1 |
| 6724383 | Herken et al. | Apr 2004 | B1 |
| 6771840 | Ioannou et al. | Aug 2004 | B1 |
| 6911995 | Ivanov et al. | Jun 2005 | B2 |
| 7065461 | Chang et al. | Jun 2006 | B2 |
| 7203342 | Pedersen | Apr 2007 | B2 |
| 7215810 | Kaufmann et al. | May 2007 | B2 |
| 7239751 | Amador | Jul 2007 | B1 |
| 7605817 | Zhang et al. | Oct 2009 | B2 |
| 7639896 | Sun et al. | Dec 2009 | B2 |
| 7643966 | Adachi et al. | Jan 2010 | B2 |
| 7956862 | Zhang et al. | Jun 2011 | B2 |
| 20030067461 | Fletcher et al. | Apr 2003 | A1 |
| 20040183905 | Comaniciu et al. | Sep 2004 | A1 |
| 20040234136 | Zhu et al. | Nov 2004 | A1 |
| 20050114059 | Chang et al. | May 2005 | A1 |
| 20060221072 | Se et al. | Oct 2006 | A1 |
| 20060256114 | Nielsen et al. | Nov 2006 | A1 |
| 20070217682 | Motomura et al. | Sep 2007 | A1 |
| 20070234230 | Pedersen | Oct 2007 | A1 |
| 20070285217 | Ishikawa et al. | Dec 2007 | A1 |
| 20080013836 | Nakamura et al. | Jan 2008 | A1 |
| 20080096152 | Cheang | Apr 2008 | A1 |
| 20100053191 | Chang et al. | Mar 2010 | A1 |
| 20100074473 | Kotaba | Mar 2010 | A1 |
| 20100104199 | Zhang et al. | Apr 2010 | A1 |
| 20100111370 | Black et al. | May 2010 | A1 |
| 20100284572 | Lukas et al. | Nov 2010 | A1 |
| Number | Date | Country |
|---|---|---|
| 1780678 | May 2007 | EP |
| 2026279 | Feb 2009 | EP |
| 2166375 | Mar 2010 | EP |
| 2249311 | Nov 2010 | EP |
| 2405776 | Mar 2005 | GB |
| 11325833 | Nov 1999 | JP |
| 0108098 | Feb 2001 | WO |
| 2007056768 | May 2007 | WO |
| Number | Date | Country | |
|---|---|---|---|
| 20110274343 A1 | Nov 2011 | US |
