velocity controllers in accordance with embodiments of the invention enable velocity estimation for tracked objects. One embodiment includes a tracker controller, including: a processor; and a memory containing: a velocity tracker application; a state space describing relationships between measured locations, calculated locations, and changes in locations, where the calculated locations in the state space correspond to unoccluded points on the surface of the tracked object; wherein the processor is configured by the velocity tracker application to: pre-process the state space to identify a tracked object; estimate a velocity of the tracked object using a location history calculated from the measured locations of the tracked object within the state space and a motion model calculated from the state space; and return the velocity of the tracked object.
|
1. A tracker controller, comprising:
a processor; and
a memory containing:
a velocity tracker application;
a state space describing relationships between measured locations, calculated locations, and changes in locations, where the calculated locations in the state space correspond to unoccluded points on the surface of the tracked object;
wherein the processor is directed by the velocity tracker application to:
pre-process the state space to identify a tracked object;
estimate a velocity of the tracked object using a location history calculated from the measured locations of the tracked object within the state space and a motion model calculated from the state space by:
dividing the state space into a grid of cells where the grid of cells comprises a plurality of cells;
calculating a probability that the tracked object is located in a given cell with respect to each cell in the plurality of cells;
subdividing each cell in the plurality of cells having a probability that the tracked object is within the cell that satisfies a threshold into a plurality of subdivided cells; and
returning the grid of cells and the probability that the tracked object is within each of the cells in the plurality of cells; and
return the velocity of the tracked object.
2. The tracker controller of
3. The tracker controller of
4. The tracker controller of
5. The tracker controller of
6. The tracker controller of
7. The tracker controller of
8. The tracker controller of
9. The tracker controller of
10. The tracker controller of
11. The tracker controller of
where t, i, and j indicate positions in the state space, z is the measured location, x is the change in location, η is a constant, and k is a smoothing term.
12. The tracker controller of
Where t, i, and j indicate positions in the state space, z is the measured location, x is the change in location, η is a constant, k is a smoothing term, ps is the probability of a spatial match, pc is the probability of a color match, and p(V) is the prior probability of sampling from a previously visible surface.
13. The tracker controller of
14. The tracker controller of
16. The tracker controller of
17. The tracker controller of
18. The tracker controller of
19. The tracker controller of
20. The tracker controller of
|
The present invention is a continuation of U.S. patent application Ser. No. 14/733,902, entitled “Robust Anytime Tracking Combining 3D Shape, Color, and Motion with Annealed Dynamic Histograms” to Held et al., filed Jun. 8, 2015, which claims priority to U.S. Provisional Application No. 62/009,238 entitled “Method and System for Combining 3D Shape. Color, and Motion for Robust Anytime Tracking” to Held et al., filed Jun. 8, 2014. The disclosures of which are herein incorporated by reference in their entirety.
This invention was made with Government support under N00014-07-1-0747 awarded by ONR, W911NF-10-2-0059 awarded by the U.S. Army, and N00014-10-1-0933 awarded by ONR. The Government has certain rights in this invention.
The present invention relates generally to machine vision systems and more specifically to systems and methods for tracking objects.
As computers evolve, they can interact with their environment in increasingly complex ways. For example, cameras can send one or more video streams of data to a computer. These streams of data contain a myriad of information that can be further processed and used for many areas of computing including (but not limited to) object tracking, path planning, pattern recognition, scene reconstruction, object pose estimation, and image restoration.
Object tracking can require a computer (or robot) to locate a moving object and estimate its velocity in an environment. The object can be tracked by finding the same object in subsequent video frames. Real time object tracking can require this process to occur at high speeds. Real time systems generally aim to provide a response within a specified amount of time to enable a computer to react to changes in the environment as soon as they occur. In general, robotics applications including autonomous vehicles utilize perception systems that are real time because a car accident can occur if a car fails to make quick decisions.
Velocity controllers in accordance with embodiments of the invention enable velocity estimation for tracked objects. One embodiment includes a tracker controller, including: a processor; and a memory containing: a velocity tracker application; a state space describing relationships between measured locations, calculated locations, and changes in locations, where the calculated locations in the state space correspond to unoccluded points on the surface of the tracked object; wherein the processor is configured by the velocity tracker application to: pre-process the state space to identify a tracked object; estimate a velocity of the tracked object using a location history calculated from the measured locations of the tracked object within the state space and a motion model calculated from the state space; and return the velocity of the tracked object.
In a further embodiment, changes in locations in the state space are measures of changes in position relative to a last observation.
In another embodiment, the state space further comprises three-dimensional information.
In a still further embodiment, the state space further comprises two-dimensional information.
In still another embodiment, the state space further comprises a Dynamic Bayesian Network.
In a yet further embodiment, the measured locations in the Dynamic Bayesian Network are independent of previous measured locations and velocities and the changes in locations in the Dynamic Bayesian Network are measures of changes in position relative to a last observation.
In yet another embodiment, pre-processing the state space further comprises a point-cloud based segmentation process.
In a further embodiment again, estimating a velocity of a tracked object using the location history and the motion model further comprises an annealed dynamic histogram (ADH) process.
In another embodiment again, the ADH process further comprises: dividing the state space into a grid of cells where the grid of cells comprises a plurality of cells; calculating a probability that the tracked object is located in a given cell with respect to each cell in the plurality of cells; subdividing each cell in the plurality of cells having a probability that the tracked object is within the cell that satisfies a threshold into a plurality of subdivided cells; updating the grid of cells with the plurality of subdivided cells; and return the grid of cells and the probability that the tracked object is within each of the cells in the plurality of cells.
In a further additional embodiment, the ADH process is adaptable based upon a desired runtime and a desired tracking resolution.
In another additional embodiment, the probability that the tracked object is in a given cell is evaluated using the location history.
In a still yet further embodiment, the location history is evaluated using the following expression:
where t, i, and j indicate positions in the state space, z is the measured location, x is the change in location, and η is a constant, and k is a smoothing term.
In still yet another embodiment, the location history further comprises optional color channel information and is evaluated using the following expression:
where t, i, and j indicate positions in the state space, z is the measured location, x is the change in location, and η is a constant, k is a smoothing term, ps is the probability of a spatial match, pc is the probability of a color match, and p(V) is the prior probability of sampling from a previously visible surface.
In a still further embodiment again, the probability that the tracked object is in a given cell is evaluated using the motion model.
In still another embodiment again, the motion model is evaluated using the following expression: p({circumflex over (x)}t,i|z1 . . . zt−1), where t and i indicate positions in the state space, z is the measured location, x is the change in location.
Another further embodiment of the method of the invention includes: the threshold is determined by a KL-divergence.
Still another further embodiment of the method of the invention includes: a number of the plurality of subdivided cells is evaluated using the following expression: k=3d, where d is the number of dimensions.
In further embodiment, the measurement model is evaluated using a kd-tree for the nearest-neighbor for each point.
In another embodiment, the measurement model is evaluated using a pre-caching process to pre-cache the probability that a tracked object is within a cell in the plurality of cells.
In a still further embodiment, the measured locations compares two frames with a smaller and a larger set of points by tracking from the frame with the smaller set of points to the frame with the larger set of points.
In still yet another embodiment, the measurement model is evaluated using a combination of a kd-tree for the nearest-neighbor for each point and a pre-caching process to pre-cache the probability that a tracked object is within a cell in the plurality of cells.
Turning now to the drawings, systems and methods for estimating velocities of tracked objects using location history and motion models in accordance with embodiments of the invention are illustrated. Real time object tracking can be a complex problem involving a number of challenges including (but not limited to) changes in viewpoint of the camera, occlusions, and/or lighting variations. In addition, objects being tracked may not be uniform and can have differences such as varied shape, varied size, and/or varied distance from the camera. Occlusions can occur when all or part of a tracked object moves behind another object out of the field of view of the camera. In general, changes in viewpoint, occlusions, and/or lighting variations, as well as nonuniform tracked objects can generate noise in real time object tracking calculations. Tracking three dimensional (3D) objects can decrease this noise.
To track a 3D shape, real time object trackers in accordance with many embodiments of the invention can form a model of the tracked objects. One method to model tracked objects is a Bayesian probabilistic framework using a Dynamic Bayesian Network, which can combine 3D shape information with motion data and other optional channels of information. These optional channels of information can include color channels and/or depth information. Color channels can be made up of individual and/or several color channels from various color models including RGB (red green blue), CMYK (cyan magenta yellow black), or HSV (hue saturation value). Additional channels can include (but are not limited to) channels providing depth information, and confidence information related to depth estimates. In various embodiments depth information can be obtained from any of a variety of depth sensors including (but not limited to) multiview stereo cameras, time of flight cameras, structured illumination systems, and/or Light Detection and Ranging (LIDAR) systems.
In various embodiments, 3D shape information in the Dynamic Bayesian Network model can include both a calculated surface of the 3D object and sensor measurements of the object. These values can be different because of inherent noise in sensor measurements, and the use of both values can decrease some of the effects of the sensor noise on real time object tracking velocity calculations. Additionally, a measurement model can be calculated from the Dynamic Bayesian Network model. This measurement model can provide location history of objects and the probability an object will be measured at a given location based on that location history. In various embodiments, a motion model, which can include velocity information, can also be incorporated into the real time object tracker.
Objects can be segmented from the background as a pre-processing step using various methods. In some embodiments, a point-cloud based segmentation can be used to generate segmented objects within the Dynamic Bayesian Network model. The model can then be coarsely divided and subsections can be sampled at a higher resolution to globally explore the space in real time. In many embodiments, an annealed dynamic histograms (ADH) approach can be used, which can quickly return coarse velocity values when optimized for speed, or be allowed to iterate over time and can return more accurate velocity values as the system or method requires.
Systems and methods for performing velocity estimation of tracked objects in real time and solutions to a velocity estimation real time object tracking problem that can be utilized in the implementation of such systems and methods in accordance with embodiments of the invention are discussed further below.
Tracking Objects with Varying Appearance Between Successive Frames
Many robotics applications are limited in what they can achieve due to unreliable tracking estimates. For example, an autonomous vehicle driving past a row of parked cars should know if one of these cars is about to pull out into the lane. Many object tracking processes in current use provide noisy estimates of the velocity of these vehicles, which are difficult to track due to heavy occlusion and viewpoint changes. Additionally, without robust estimates of the velocity estimates of the velocity of nearby vehicles, merging onto or off highways or changing lanes become formidable tasks. Similar issues are encountered by any robot that operates autonomously in crowded, dynamic environments.
The challenge presented to object trackers by occlusions and/or viewpoint changes when performing object tracking is conceptually illustrated in
The velocity of the tracked car can be determined by identifying a common point on the car in each of the two captures images 100, 102 and determining the motion of the image during the elapsed time. In many systems, a common point is selected by determining the centroid of the group of pixels constituting the object in each of the images. When the appearance of the object changes between images, then the centroid that is determined during the velocity estimation process will be in different locations relative to the 3D shape of the object in each image. The differences in the placement of the centroid relative to the 3D shape of the object in each image introduce a source of error in the velocity measurement. As discussed further below, systems and methods in accordance with various embodiments of the invention determine velocity accounting for variation in the appearance of an object based upon factors including (but not limited to) occlusion and/or changing viewpoint. In many embodiments, a motion model is utilized that determines the probability of a specific motion given observed points associated with the surface of a 3D object. In several embodiments, the probability of an observed motion considers the probability of correspondence between observed points visible in successive images of a scene. Various motion models that can be utilized in accordance with embodiments of the invention are discussed further below.
As can readily be appreciated, the tree shown in
Robotic Software Architectures
A software architecture that can be utilized in an autonomous vehicle is illustrated in
Tracker Controllers
A tracker controller in accordance with an embodiment of the invention is shown in
Although a number of different tracking controllers are described above with respect to
Tracking Pipeline
A process that can be performed by a tracker controller to estimate velocities for tracked objects in accordance with an embodiment of the invention is illustrated in
In many embodiments, a point-cloud based segmentation and data association algorithm can be used as a pre-processing step, which can segment objects from the background into clusters and associate these object clusters between successive time frames. The tracking method can then estimate the velocity of each of the pre-segmented tracked objects. The annealed dynamic histograms technique, which is described in detail further below, can globally explore the search space in real-time. The state space is sampled with a coarse grid, and for each sample, the probability of the state using a measurement model can be calculated. The grid of cells can then subdivided to refine the distribution. Over time, the distribution becomes increasingly accurate. After the desired runtime or tracking resolution, the tracker can be stopped, at which point the current posterior distribution over velocities can be returned.
State Space
In various embodiments, a probabilistic model can be used for tracking. As described below, this model can be used as part of an annealed dynamic histogram framework. xt is a state variable which can be defined as xt=(xt,p,{dot over (x)}t), where xt,p can be a linear position and {dot over (x)}t can be a velocity of a tracked object. Since object tracking can estimate the motion of objects, the position state variable xt,p can measure the change in position relative to the last observation. To achieve this, in some embodiments, after each observation the origin of the coordinate system can be set to a location at the centroid of the previous observation. The position state variable thus measures how far this object has moved since the previous observation.
In many embodiments, the rotational velocity of the tracked object is small relative to the frame rate of the sensor. This is often the case for people, bikes, and cars moving in urban settings. Thus, the rotational velocity is not included in the state. In various embodiments, the rotational velocity can be estimated in many ways, including by obtaining the posterior over translation and searching for the optimal rotation as described further below.
Models can be general and can be used to track objects moving in three dimensions. In many embodiments, however, objects of interest (people, bikes, and/or cars) are generally confined to the ground surface. Thus, to speed up the method, tracked objects in some embodiments exhibit minimal vertical motion within the frame rate of the sensor. In a number of embodiments, therefore, the state space can be limited to only model motion along the ground surface, which can result in a significant processing speedup, with minimal effect on accuracy. In other embodiments such as (but not limited to) drones and other flying robots, no assumptions are made with respect to objects of interest having constrained motion. In various embodiments, moving objects can be tracked vertically by appending the vertical dimension to the state space, which can result in a slightly slower method. Alternatively, an elevation map can be incorporated to predict vertical motion due to elevation changes. The specific motion model utilized when performing motion tracking typically depends upon the requirements of a given application.
Dynamic Bayesian Network
A Dynamic Bayesian Network upon which a motion model can be built for use in object tracking in accordance with an embodiment of the invention is shown in
The latent surface variable st is related to similar notions in other systems. For example, some systems include a geometry variable in which they model objects as 2D rectangles and estimate their width and length. SLAM systems use a similar variable to represent the environment map. Both of these methods attempt to explicitly model the geometry of the tracked object or environment. In contrast, various embodiments of the present invention may not explicitly model the object's shape, but can integrate over shapes and focus on estimating the target object's velocity.
The latent surface variable st can be represented as a collection of n points {st,1 . . . st,n}ϵst sampled from the visible surface of the tracked object at time t. The prior p(st) on these points can be a uniform distribution over the maximum size of a tracked object. This prior can decompose as a product of the priors for each point in st, i.e. p(st)=Πjp(st,j).
In several embodiments, the measurement zt can represent the set of n observed points at time t, {zt,1 . . . zt,n}ϵzt. Each measurement zt,j can be drawn from a corresponding latent surface point st,j. Because of sensor noise, the observed measurements zt will not lie exactly on the object surface and hence will not be exactly equal to st. The observed points zt can be generated from the latent surface st and the state xt via the following procedure: for each latent surface point st,j, Gaussian noise can be added based on the sensor resolution Σe to create a noisy point {tilde over (s)}t,j. The point {tilde over (s)}t,j can be shifted according to the current object position xt,p to generate the measurement zt,j at the appropriate location. Therefore, each measurement zt,j can be written as:
zt,j˜(st,j,Σe)+xt,p. (1)
The previous measurements zt−1 can be centered on the origin of the coordinate system. The points in zt−1 can be noisy observations of the previous surface st−1. Therefore, for each point zt−1,iϵzt−1 from the previous observation:
zt−1,i˜(st−1,i,Σe). (2)
In various embodiments, an additional conditional independence assumption can be created which is not encoded in the graphical model:
p(zt−1|xt,st−1)=p(zt−1|st−1). (3)
where p(st,|st−1) can represent the probability of sampling points st from the currently visible object surface given the previously sampled points st−1. The sampled points may have changed due to occlusions, viewpoint changes, deformations, and random sampling. Additionally, in some embodiments, every point st,jϵst could have either been generated from a previously visible portion of the object surface at time t−1 or from a previously occluded portion. If p(V) represents the prior probability of sampling from a previously visible surface, then:
p(st,j|st−1)=p(V)p(st,j|st−1,V)+p(V)p(st,j|st−1,
V) (4)
p(st,j|st−1,V) can be modeled as a Gaussian, st,j˜(st−1,i,Σr) where Σr can model the variance resulting from the sensor resolution as well as from object deformations, and st−1,i can be the nearest corresponding (latent) surface point from the previous frame. The sensor resolution can change as a function of distance, and Σr can be calculated accordingly for each tracked object.
The probability that st,j is generated given that the surface from which it is sampled was previously occluded can be represented by p(st,j|st−1, V). In numerous embodiments, an occlusion model for the previous frame can be used to calculate this probability. Otherwise, in many embodiments, any region that was not previously visible can be assumed to be previously occluded. This can be generically written as:
p(st,j|st−1,V)=k1(k2−p(st,j|st−1,V))
for some constants k1 and k2. In many embodiments, equation 4 can be simplified as
p(st,j|st−1)=η(p(st,j|st−1,V)+k) (5)
where η can be a normalization constant, k can act as a smoothing factor, and
η=p(V)−p(V)k1
k=p(V)k1k2/η.
The sampling process is illustrated in
Tracking
The Dynamic Bayesian Network described above can be used to track moving objects and estimate their velocity. p(xt|z1 . . . zt), the probability of the state xt given the past observations, can be rewritten using Bayes' rule as
p(xt|z1 . . . zt)=ηp(zt|xt,z1 . . . zt−1)p(xt|z1 . . . zt−1) (6)
where η can be a normalization constant. The first term can be a measurement model. In a standard Bayes filter algorithm, conditional independence assumptions can be used to simplify this as
p(zt|xt,z1 . . . zt−1)=p(zt|xt).
However, in many embodiments, this simplification can be prevented by the latent variables s1, . . . st. Because all observations come from the same object surface, they cannot be considered independent of each other. Therefore, in various embodiments, a slightly different approximation can be made:
p(zt|xt,z1 . . . zt−1)≈p(zt|xt,zt−1) (7)
Intuitively, the current observation can be most strongly affected by the previous observation, rather than the entire past history of observations. Although tracking could be improved by using the entire past history of observations, this would add a computational cost that can be avoided. The second term on the right-hand side of equation 6 can be obtained from the motion model, which will be described in detail below.
Measure Model Derivation
Measurement models that can be utilized in a variety of embodiments of the invention are described below that can be derived using a Dynamic Bayesian Network similar to the Dynamic Bayesian Network discussed above with reference to
where the chain rule of probability and the conditional independence assumptions can be used from the Dynamic Bayesian Network. The second term inside the integral can be further expanded as
p(st|xt,zt−1)=∫p(st,st−1|xt,zt−1)dst−1 (9)
In various embodiments, independence assumptions from the Dynamic Bayesian Network as well as the independence assumption from equation 3 can be used to further expand the term inside this integral as
where η can be a normalization constant. p(st−1) can be a constant and therefore can be absorbed by the normalization constant r). The next two terms can be given by equations 2 and 5.
p(zt−1|st−1)=(zt−1;st−1,Σe)
p(st|st−1)=η((st;st−1,Σr)+k)
where η can be a normalization constant and k can be a smoothing term. In several embodiments, the integral in equation 9 can be evaluated as
p(st|xt,zt−1)=η((st4;zt−1,Σr+Σe)+k)
using the fact that the convolution of two Gaussians is another Gaussian. Additionally, equation 1 provides that
p(zt|st,xt)=(zt;st+xt,p,Σe)
In some embodiments, the integral of equation 8 can be evaluated once again using the fact that convolution of two Gaussians is another Gaussian to generate
p(zt|xt,zt−1)=η((zt;zt−1+xt,p,Σr+2Σe)+k).
In numerous embodiments, the measurement model can be calculated by letting
where η can be a normalization constant and k can be a smoothing factor. The covariance matrix Σ can be given by Σ=2Σe+Σr, with covariance terms for Σe due to sensor noise and Σr due to the sensor resolution. To perform this calculation, the previous points zt−1 can be shifted by the proposed velocity xt to obtain the shifted points
Tracking with Color
In many embodiments, a 3D-based model can be used when no color information is available. However, if color is available (such as from a video camera), color matches can be incorporated into the probabilistic model. To leverage color, the probability distribution over color for correctly aligned points can be learned. To do so, a large dataset can be built of correspondences (with in some embodiments, a 5 cm maximum distance) between colored laser returns from one laser spin and each of their spatially nearest points from the subsequent spin, aligned using recorded ego motion. By observing how the color of this point changes as it is moved past, the probability distribution for color changes over a single frame can be learned
A normalized histogram can be built of the differences in color values between each point and its closest neighbor from the next spin. In some embodiments, the distribution closely follows a Laplacian distribution.
In various embodiments, multiple color channels can be incorporated into the model. However, such a model would require learning the covariances between different color channels. In many embodiments, the model can be simplified by incorporating just a single color channel (for example, blue), chosen using a hold-out validation set. Although adding other color channels can provide additional benefit, improved tracking performance can be shown with just one color channel.
The learned color distribution can be incorporated into the measurement model described above. The term p(st,j|st−1,V) from equation 4 can represent the probability of sampling point st,j given that the surface from which it is sampled was previously visible at time t−1. This term can be expanded as a product of spatial and color probabilities:
p(st,j|st−1,V)=ps(st,j|st−1,V)pc(st,j|st−1,V) (12)
where ps(st,j|st−1,V) can be the probability based on the spatial match with the points in st−1, and pc(st,j|st−1,V) can be the probability based on the color match with the points in st−1. As described above, the spatial match probability can be modeled as ps(st,j|st−1,V)=(st,j;st−1,i,Σr), where Σr models the variance resulting from the sensor resolution as well as from object deformations and st−1,i is the nearest corresponding (latent) surface point from the previous frame.
In several embodiments, for the color match probability, due to changes in lighting, lens flare, or other unmodeled causes, the observed colors of an entire image can change drastically between two frames. Therefore, there can be some probability p(C) that all of the observed colors in an image will change in a way that is not modeled by the learned color distribution. The color match probability from equation 12 can be written as
pc(st,j|st−1,V)=p(C)pc(st,j|st−1,V,C)+p(C)pc(st,j|st−1,V,
C) (13)
where pc(st,j|st−1,V,C) can be the learned color model distribution (parameterized as a Laplacian) and pc(st,j|st−1,V,C)= 1/255 can be the probability of a point having any color, given that the color does not need to match to that of a nearby point from the previous frame.
The probability that the color should match between two aligned points, p(C), must be chosen with care. In many embodiments, when coarsely sampling the state space as described in detail below, colors are not expected to match very well. Therefore, p(C) can be set to be a function of the sampling resolution as
where r can be the sampling resolution and σc can be a parameter that controls the rate at which p(C) decreases with increasing resolution. Therefore, when sampling is coarse, p(C) can have a smaller value and colors are not expected to match at a coarse resolution. As sampling becomes more fine, p(C) can increase until p(C)=pc when r=0, so colors can match more precisely at a finer sampling resolution.
In various embodiments, the measurement model can be computed incorporating color as follows: As described before, let
ps(zj|xt,zt−1)=exp(−½(zj−
The color probability can be calculated as
pc(zj|xt,zt−1,V)=p(C)pc(zj|C)pc(zj|
C)
where p(C) can be given by equation 14, pc(zj|distribution (parameterized as a Laplacian), p(
C)=1−p(C), and pc(zj|
C)= 1/255. Finally, the total measurement probability is calculated as
where k3 can be calculated from k in equation 11 as k3=k/(k+1). The parameter k4 can be a smoothing parameter that can be chosen via cross-validation. In various embodiments, k4 can be set to 1.
Motion Model
A motion model can be incorporated into the tracker. In several embodiments, adding a motion model significantly improves tracking performance. To build the motion model, the values for p(xt|z1 . . . zt) from the previous frame can be fit to a multi-variate Gaussian to the set of probabilities. The mean μt and covariance Σt can be calculated by weighting each state by its probability as
where xt,i can be the state vector of sample i. Once a Gaussian over the posterior is obtained, the result can be used in the standard constant velocity model of a Kalman filter.
Implementation of Tracking
In some embodiments, the measurement model can be calculated by using a kd-tree to search for the nearest-neighbor for each point. The space can be divided into a grid and the log probability for each grid cell after each search can be cached. Thus, for each grid-cell, only a single search is performed in the kd-tree. A quick table lookup of the cached result can be performed. The discretization of the measurement grid can be set equal to the state-space sampling resolution as described in detail below.
Various other embodiments can use a pre-caching technique. The space is divided into a grid as above, and for each point ziϵzt−1, the probability of a point from zt falling into such a grid cell can be pre-cached using the measurement model of equation 11. Because all points for a given object have the same covariance Σ, much of the work for this computation can be done once and then reused for all points. Using pre-caching can avoid having to perform relatively costly kd-tree searches.
Additionally, it is noted that equation 11 is not symmetric with respect to zt and zt−1, especially if the two point clouds are different sizes. Specifically, suppose that zt is much larger than zt−1 due to points becoming unoccluded (or remaining occluded but less of the object remaining occluded). In such a case, a large penalty can be incurred for each point in zt that does not have a nearby match in zt−1. The tracker can focus on aligning the densest part of zt in order to minimize the number of unmatched points. This can lead to an incorrect alignment estimate. In many embodiments, the tracker can rectify this problem by choosing the larger of zt and zt−1 (in terms of the number of points) to play the “role” of zt−1 and the smaller point cloud will play the “role” of zt in equation 11. Thus, as described above, the space can be divided into a grid, the larger of zt and zt−1 can be pre-cached in this grid. Each point can be iterated over from the smaller of zt and zt−1 and probabilities can be looked up in the grid to calculate the measurement model of equation 11. By performing the computation in this manner, performance improvements may be obtained. It is important to remember, when implementing these embodiments, that the resulting alignment will give the opposite of the actual velocity if zt−1 and zt were switched to perform this computation. In various embodiments, a hybrid approach can be achieved by combining aligning the roles of smaller and larger point clouds with using a kd-tree to search for the nearest-neighbor for each point.
Annealed Dynamic Histograms
In several embodiments, using the techniques described above, the measurement model and motion model probabilities can be very quick to compute for any particular candidate alignment between frames. However, these probabilities are calculated separately for every state xt considered. If the state space is densely sampled, then there will be a large number of computations to perform, potentially rendering this method too slow for real-time use on certain computing platforms. In order to enable the method to globally explore the state space in real-time, a technique called annealed dynamic histograms can be utilized. Processes which can utilize annealed dynamic histograms are discussed further below.
Derivation of Annealed Dynamic Histograms
A process that utilizes annealed dynamic histograms to perform motion tracking in accordance with an embodiment of the invention is illustrated in
An overall approach to dynamic histograms can be visualized in
As described above, the state space is divided into a coarse grid. Some cells can then be subdivided into k sub-cells, based on a criteria to be established below. R can be the (possibly non-contiguous) set of all cells chosen to subdivide into sub-cells ci. The discrete probability of the sub-cell ciϵR can be calculated as follows:
where |ci| can be the volume of sub-cell ci. Therefore, it can be shown that ΣiϵRp(ci)=p(R). The key here is that the normalization constant η depends only on other sub-cells in region R. Thus, the probability values of all cells outside of region R are unaffected by this computation.
In many embodiments, the criteria for choosing which cells to divide can now be derived, based on minimizing the KL-divergence between the current histogram and the true posterior. Suppose distribution B can be the current estimated distribution before a given grid cell can be divided, and distribution A can be the new estimated distribution after the grid cell can be divided into k sub-cells. In distribution A, the k new sub-cells can each take on separate probabilities, allowing more accurate approximations of the true posterior. The KL-divergence between distributions A and B can measure the difference between the old, coarser approximation to the posterior and the new, improved approximation to the posterior. The size of the KL-divergence indicates how much the approximation can improve to the posterior by dividing a given grid cell.
In numerous embodiments, a cell can have a discrete probability Pi, and suppose a decision is being made whether to divide this cell. Before dividing, the cell can be viewed as having k sub-cells, each of whose probability is constrained to be Pi/k. After dividing, each of the sub-cells can take on a new probability pj, with the constraint that Σj=1kpj=Pi. This situation is shown in
The KL-divergence between these two distributions can be computed as
where B is the distribution before dividing and A is the distribution after dividing. The KL-divergence will obtain its maximum value if pj′=Pi for some j′ and pj=0 for all j≠j′. In this case, it can be shown that
DKL(A∥B)=Pi ln k (15)
If the computation for each of the k sub-cells takes t seconds, then the maximum KL-divergence per second is then equal to Pi ln k/(kt). If the goal is to find the histogram that matches as closely as possible to the true posterior, then all cells whose probability Pi exceeds some threshold pmin can simply be divided. Similarly, to achieve the maximum benefit per unit time, k can be chosen to be as small as possible. In many embodiments, for a histogram in d dimensions, k=3d can be used, splitting cells into thirds along each dimension.
Annealing
In several embodiments, when the state space is sampled at a very coarse resolution, it is not expected to find a good alignment between the previous frame and the current frame for the tracked object. In fact, the sampling resolution (shown in
Using the Tracked Velocity Estimates
In several embodiments, the tracker can return information about the tracked object's velocity in any format, as requested by the planner or some other component of the system. For example, to minimize the RMS error, the mean of the posterior can be returned. On the other hand, to build accurate object models, the tracker can return the mode of the distribution. A simple planner might request either the mean or the mode of the posterior distribution, as well as the variance. A more sophisticated planner could make use of the entire tracked probability histogram, in the form of a density tree.
The mean of the distribution can minimize the RMS error. In various embodiments, the true distribution over velocities can be p(v), where the distribution is based on the probabilistic noise in the measurement (from the sensor noise and the sensor resolution). To minimize the RMS error, a velocity {circumflex over (v)} can be found that minimizes
where the expectation can be taken with respect to p(v). To minimize this, the expectation can be rewritten as a sum:
The derivative can then be taken with respect to {circumflex over (v)} and set to 0 to get
where p(v) can be a probability distribution, so Σvp(v)=1. The expression Σvp(v)v is simply the mean of v, μv, thus demonstrating that the mean of the distribution, rather than the mode, can minimize the RMS error. In various embodiments, depending on which objective is important for a particular application, the tracker can return the mean, the mode, and/or the entire distribution, as described above.
Estimating Rotation
In numerous embodiments, the rotation of the tracked object can be estimated in addition to the translation. To estimate the rotation, the mode of the posterior distribution over the translation is found. A coordinate descent can be performed in each rotational axis, holding translation fixed. Based on the application, yaw, roll, and pitch can be searched over individually. Because each axis of rotation can be separately searched, this optimization can be relatively quick to perform.
In various embodiments, rotation can be estimated by the tracker in a variety of ways depending on the application. The tracker can first estimate the translation and then estimate the rotation. Additionally, the tracker can alternate between estimating the translation and estimation the rotation. When estimating the rotation, the tracker can perform coordinate descent, estimating the rotation one dimension at a time. Alternatively, the tracker can jointly estimate the rotation simultaneously over all rotational directions. In addition, the tracker can separately estimate the translation for a number of different possible rotations. Furthermore, the tracker can jointly estimate the translation and rotation together in a joint 6-dimensional state space.
System Simulation
A 3D point cloud for object tracking simulations is obtained with a Velodyne HDL-64E S2 LIDAR mounted on a vehicle. The Velodyne has 64 beams that rotate at 10 Hz, returning 130,000 points per 360 degree rotation over a vertical range of 26.8 degrees. These points are colored with high-resolution camera images obtained from 5 Point Grey Ladybug-3 RGB cameras, which use fish-eye lenses to capture 1600×1200 images at 10 Hz. The laser is calibrated using standard techniques. The vehicle pose is obtained using the Applanix POS-LV 420 inertial GPS navigation system. Simulations were performed single-threaded on a 2.8 GHz Intel Core i7 processor.
Results of Simulations
The simulated tracker is evaluated on a large number of tracked objects. Parked cars with a local reference frame in which they appear to be moving are tracked which allows the accuracy of the velocity measurements to be directly measured.
Choosing Parameters for Simulations
Parameters for the simulated model, as well as parameters for the baseline methods for comparison, were chosen by performing a grid search using a simulated training set that is completely separate from the simulated test set. The simulated training set consists of 13.6 minutes of logged data, during which time a total of 662 parked cars were driven past. Using this simulated training set, the final parameters chosen for the system are as follows, for an object with a horizontal sensor resolution of r meters and a state-space sampling resolution of g: k=0.8, Σe1=2Σe=(0.03 m)2I, Σr=(r/2)I, and Σg=gI, where I is the 3×3 identity matrix. For the dynamic histogram, a coarse resolution of 1 m is initialized, and the simulated tracker continued until the resolution of the resulting histogram is less than max(r, 0.05 m), with pmin=10−4. For the color model, pc=0.05 and σc=1 m were chosen. Because the data contains many frames with lens flare as well as underexposed frames, the simulated system learns to assign a low priority to color matches. The learned color distribution for aligned points is given by a Laplacian with parameter b=13.9. For all methods tested, the current frame's point cloud is downsampled to no more than 150 points and the previous frame to no more than 2000 points, where the current frame zt and previous frame zt−1 are chosen as described above.
Evaluation of the Dataset of the Relative Reference Frame
In many embodiments, an approach to quantitatively evaluate the results of the simulated tracker is to track parked cars in a local reference frame in which they appear to be moving. In a 6.6 minute test dataset, 557 parked cars were driven past. However, in a local reference frame, each of these parked cars appears to be moving in the reverse direction of the simulated tracker's motion. Because the simulated tracker logs velocity, the ground-truth relative velocity of a parked vehicle in this local reference frame can be computed to the precision of simulated tracking velocity estimates. Furthermore, each car is driven past, the viewpoint and occlusions can change over time, which enables evaluation of the simulators response to many real-world challenges associated with tracking. Further simulated quantitative results when tracking moving objects of different classes (cars, bikes, and pedestrians) will be shown below, to demonstrate that the method works on moving objects and generalizes across object types.
During the 6.6 minutes of the simulated test set, the trajectory follows the path shown in
Tracks that contain major undersegmentation issues can be ignored, in which the tracked object is segmented together with another object as this can lead to an ambiguity about the correct ground-truth alignment. Therefore, 7% of the simulated initial tracks are filtered out based on segmentation issues, leaving 515 properly segmented cars to track. In some embodiments, tracks in which the tracked object has been oversegmented into multiple pieces are not filtered out.
Baseline Comparison for Simulations
To compare the robustness of different tracking methods, the RMS tracking error is calculated for each method. The results are shown in
A Kalman filter can be evaluated with a measurement model given by the centroid of the tracked points. Because of its speed and ease of implementation, this is a popular method. As can be seen in
A number of variants of the iterative closest point method (ICP) can be evaluated. The basic point-to-point ICP algorithm can be evaluated. As is standard, ICP can be initialized by aligning the centroids of the tracked object. ICP is simulated to run for 1, 5, 10, 20, 50, and 100 iterations, and the results are shown in
A Kalman filter with ICP used as the measurement model can also be compared. Combining ICP with the motion model in a Kalman filter makes the method much more robust to failures of ICP. Because ICP is dependent on its initialization, three different strategies to initialize this method are simulated. ICP can be initialized by aligning the centroids of the tracked object, by using the mean prediction from the motion model, and by first running the centroid through a Kalman filter and using the output as the initialization for ICP.
Using a Kalman filter with ICP initialized by aligning the centroids, performs reasonably, with an improvement of 11.7% over a simple Kalman filter. Finally, running the centroid through a Kalman filter and using the output as the initialization for ICP (and using the ICP result as the measurement model for another Kalman filter) generates the best performance of the ICP baseline methods that were simulated, with an RMS error of 0.63 m/s. However, simulated versions of ICP suffer from the problem of choosing a good initialization. The simulated tracker, in contrast to all of the ICP methods, is more robust in that it does not depend on the initialization.
The simulated tracker achieves an accuracy of 0.53 m/s after running for just 0.37 milliseconds. Even at this minimum runtime, this performs 32.7% better than a simple Kalman filter and 25% better than all of the baseline methods of similar speed, as shown in
In various embodiments, if color images are available, then the measurement model can easily be extended to incorporate color as described above. With the addition of color, the simulated RMS error decreases by an additional 10.4%, achieving an RMS error of 0.43 m/s. Note that many frames in the simulated test set contain heavy shadows or lens flare. Therefore, color can be expected to make an even bigger difference when lens flare and similar exposure problems can be avoided.
It is interesting to compare the performance of the simulated tracker to that of a radar, which can also be used to measure the velocity of moving objects. The Bosch LLR3 Radar can estimate velocities to within 0.12 m/s. However, a radar only estimates velocity in a single dimension: in the direction from the radar to the tracked object. This one-dimensional estimate is of limited use for robotics or autonomous driving, in which a 2D estimate of the velocity of each object is preferred in order to estimate where each object is moving. The simulated tracker returns a 2D velocity estimate, and by searching over vertical motion and over rotations the simulation can be made to return a 6D velocity estimate.
In some embodiments, the simulation can be modified to align the smaller set of points to the larger set of points, as explained above, instead of always aligning the more recent points zt to the points from the previous frame zt−1. By aligning the smaller set of points to the larger set, a 10% improvement in RMS error can be achieved, from 0.54 m/s to 0.49 m/s. In other embodiments, combining this with a simulated tracker that incorporates color can generate an even bigger improvement at 12.6%, from 0.50 m/s RMS error to 0.43 m/s.
In various embodiments, a fast pre-caching scheme to compute the measurement model as opposed to kd-tree searches. The resulting simulated tracker is almost 3 times faster at the same level of resolution, reducing the speed from 1.8 milliseconds per frame to 0.64 milliseconds per frame at a sampling resolution of 3.7 cm. By avoiding kd-tree searches and re-using the covariance computations for each point, a significant speedup can be obtained.
Sample Analysis
One of the novel additions of the simulated tracker method is the use of annealed dynamic histograms, as described in above, to speed up the tracker.
A multi-resolution model can also be compared. A multi-resolution model that computes a low-resolution model (0.3 m) and a high-resolution model (0.03 m) is simulated. The low-resolution model is computed by taking a maximum over the high-resolution regions. Sampling then alternates between the low and high resolution models until the method has found the maximum of the high resolution model. Due to the way in which the low resolution model is constructed, this model is guaranteed to find the global maximum of the high resolution model. Indeed, the search space can be sampled in the multi-resolution method as describe above and the mean of the resulting distribution is taken and a resulting RMS accuracy of 0.49 m/s can be achieved, which is the same accuracy achieved by sampling with the simulated ADH tracker.
However, the multi-resolution sampling method can be much slower than the sampling method of the simulated ADH tracker. The simulated ADH tracker takes about 0.64 ms per object per frame, whereas the simulated multi-resolution tracker takes 14.5 ms per object per frame, or a 24 times reduction in speed. The main reason that the multi-resolution tracker is slower is that it requires more samples from the state space to find the mode, requiring an average of 3899 samples, whereas the simulated ADH tracker uses an average of only 172 samples. The simulated ADH tracker thus requires 23 times fewer samples, which almost completely accounts for the difference in runtime. Furthermore, if a hard limit is placed on the number of samples taken by the multi-resolution tracker, the error rate increases dramatically, indicating that early-stopping to decrease the runtime is not a viable option for the multi-resolution tracker.
There are multiple differences between the sampling methods of the simulated ADH tracker and that of the simulated multi-resolution tracker. One difference is that the ADH tracker can sample at many levels of resolution. In the simulated implementation, the ADH tracker begins with a coarse sampling of 1 m. The tracker then repeatedly reduces the sampling resolution by a factor 3, thus sampling at 0.33 m, then 0.11 m, and finally at 0.03 m. In contrast, the two level multi-resolution tracker samples at only 2 resolutions: first a coarse sampling at 0.3 m, followed by a fine sampling at 0.03 m.
To understand how important this difference was, the ADH tracker was modified so that it also samples only at 2 resolutions: a coarse sampling of 0.3 m followed by a fine sampling of 0.03 m, just like the simulated two level multi-resolution tracker. Despite this change, the ADH tracker is still more efficient, running in 2 ms, or a speedup of a factor of 7 over the two level multi-resolution tracker. This speedup factor of 7 must then be based on a fundamental difference in the sampling method between the two level multi-resolution tracker and ADH trackers. It can be concluded that the ADH tracker gains a 3.3 times speedup over the multi-resolution tracker by using a 4-level resolution model rather than a 2-level resolution model, and the ADH tracker gains an additional 7 times speedup based on the method of sampling alone.
The cause of the factor of 7 speedup over the multi-resolution tracker is based on the way in which samples are generated. To sample at a coarse resolution, the multi-resolution tracker takes a maximum over the corresponding high resolution regions. In contrast, the ADH tracker increases the noise of the measurement model by an amount proportional to the sampling resolution. Taking a maximum as in the multi-resolution tracker provides an extreme level of smoothing and thus too many samples at the coarse resolution appear to have a high probability. In contrast, increasing the measurement noise in the ADH tracker still leads to an informative distribution at the coarse resolution, in that low probability regions at a high resolution tend to correspond to low probability regions at the coarse resolution. Thus, in the ADH tracker, the coarse resolution is more informative about which regions are most likely to contain the mode of the high resolution distribution. As a result, the ADH tracker requires fewer samples to find the mode.
A further factor of 3.3 speedup is obtained using a 4-level sampling resolution model rather than just the 2-level sampling resolution model of the multi-resolution tracker, as explained above. Unfortunately, it is not possible to simply extend the multi-resolution tracker to more than 2 levels of resolution. The multi-resolution tracker uses the coarse resolution to find the mode of the higher resolution, and the method is considered to be converged once the mode of the higher resolution is discovered. In a 4-level sampling resolution model, the lowest resolution model would continue sampling until it has found the mode of the 2nd lowest resolution model. However, this mode does not necessarily correspond to the mode of the highest resolution model, so stopping at this point would not be helpful. Further, the method is already much slower than the ADH tracker, so continuing to sample would also not be practical. Thus, the multi-resolution tracker suffers by not being extensible to more than a 2-level sampling resolution model, whereas the ADH tracker naturally extends to many levels of sampling resolution.
These results are summarized in
Simulated Error Analysis
In order to better understand the performance of the simulated tracker, the performance varies as a function of the number of observed points on the tracked object can be evaluated. The results are shown in
The RMS error as a function of the distance to the tracked object can similarly be computed, shown in
Because of the motion model, the error of the method can be expected to decrease as the number of frames in which an object is seen increases, a better estimate of the prior motion of the tracked object can be obtained. This effect is shown in
These effects can be disambiguated by looking at the result of the simulated method with and without the use of a motion model, as shown in
As discussed above, the mean of the posterior distribution will minimize the RMS error. It can be proven that the RMS error decreases by 7.5% if the mean of the distribution is used instead of the mode. In a multimodal distribution, the mode will select the single point with the highest probability. However, if two modes have similar probabilities, the mode will often be far from the ground-truth alignment. The mean, on the other hand, will consider both modes and choose an estimate in between them. As described above, using the mean typically minimize the RMS tracking error.
The effect of different components of the system can be understood by looking at
In various embodiments, the different sources of error of the evaluation method used to determine the reliability of the simulated ground-truth data can be analyzed. First, by running SLAM on the dataset, it can be shown that the position estimate has an RMS error of 1.5 mm. Because the sensor has a framerate of 10 Hz, this equates to a velocity error of 0.015 m/s. Thus, the position error can account for less than 3.5% of the error of the method, which has an RMS error of 0.43 m/s when using color. The Velodyne LIDAR, on the other hand, has reported errors of less than 2 cm, which could account for 47% of the simulated total error. However, because the Velodyne has been calibrated, the expected actual error of the Velodyne measurements should be much less.
By looking at the mean velocity error, it can further be determined if there is a bias in the simulated velocity estimates. When tracking using color, the mean velocity error is −0.03 m/s, which is 5.6% of the total error. A completely unbiased tracker would have a mean velocity error of 0, when averaged over an infinite number of tracked objects. Based on the error analysis above, it can be concluded that, if the simulated method is biased, the bias is relatively small.
In numerous embodiments, it can be interesting to compare the error of the simulated method to the half-resolution of the sensor. The laser returns of the sensor becomes increasingly sparse as the distance to the tracked object increases. The resolution can be defined as the horizontal spacing between sensor measurements at a given distance. The half-resolution of the sensor can be shown in
Although the present invention has been described in certain specific aspects, many additional modifications and variations would be apparent to those skilled in the art. It is therefore to be understood that the present invention can be practiced otherwise than specifically described without departing from the scope and spirit of the present invention including (but not limited to) estimating the velocity of multiple objects simultaneously and estimating the velocity for tracked objects in a wide variety of computing applications. Thus, embodiments of the present invention should be considered in all respects as illustrative and not restrictive. Accordingly, the scope of the invention should not be determined by the embodiments illustrated, but by the appended claims and their equivalents.
Thrun, Sebastian, Savarese, Silvio, Held, David, Levinson, Jesse
Patent | Priority | Assignee | Title |
Patent | Priority | Assignee | Title |
5537118, | Mar 30 1994 | OFFICE NATIONAL D'ETUDES ET DE RECHERCHES AEROSPATIALES | Method for tracking moving objects |
9710925, | Jun 08 2014 | The Board of Trustees of the Leland Stanford Junior University | Robust anytime tracking combining 3D shape, color, and motion with annealed dynamic histograms |
20080219509, | |||
20120106784, | |||
20140358960, | |||
20150003723, | |||
20150363940, |
Executed on | Assignor | Assignee | Conveyance | Frame | Reel | Doc |
Dec 21 2016 | HELD, DAVID | The Board of Trustees of the Leland Stanford Junior University | ASSIGNMENT OF ASSIGNORS INTEREST SEE DOCUMENT FOR DETAILS | 044660 | /0429 | |
Jun 01 2017 | THRUN, SEBASTIAN | The Board of Trustees of the Leland Stanford Junior University | ASSIGNMENT OF ASSIGNORS INTEREST SEE DOCUMENT FOR DETAILS | 044660 | /0429 | |
Jun 08 2017 | LEVINSON, JESSE | The Board of Trustees of the Leland Stanford Junior University | ASSIGNMENT OF ASSIGNORS INTEREST SEE DOCUMENT FOR DETAILS | 044660 | /0429 | |
Jun 08 2017 | SAVARESE, SILVIO | The Board of Trustees of the Leland Stanford Junior University | ASSIGNMENT OF ASSIGNORS INTEREST SEE DOCUMENT FOR DETAILS | 044660 | /0429 | |
Jul 17 2017 | The Board of Trustees of the Leland Stanford Junior University | (assignment on the face of the patent) | / |
Date | Maintenance Fee Events |
Dec 06 2021 | BIG: Entity status set to Undiscounted (note the period is included in the code). |
Dec 06 2021 | M1551: Payment of Maintenance Fee, 4th Year, Large Entity. |
Date | Maintenance Schedule |
Jun 05 2021 | 4 years fee payment window open |
Dec 05 2021 | 6 months grace period start (w surcharge) |
Jun 05 2022 | patent expiry (for year 4) |
Jun 05 2024 | 2 years to revive unintentionally abandoned end. (for year 4) |
Jun 05 2025 | 8 years fee payment window open |
Dec 05 2025 | 6 months grace period start (w surcharge) |
Jun 05 2026 | patent expiry (for year 8) |
Jun 05 2028 | 2 years to revive unintentionally abandoned end. (for year 8) |
Jun 05 2029 | 12 years fee payment window open |
Dec 05 2029 | 6 months grace period start (w surcharge) |
Jun 05 2030 | patent expiry (for year 12) |
Jun 05 2032 | 2 years to revive unintentionally abandoned end. (for year 12) |