Tracking¶
Data Association¶
-
class
MarginalAssignment
(exists_logits, assign_logits, bp_iters=None)[source]¶ Computes marginal data associations between objects and detections.
This assumes that each detection corresponds to zero or one object, and each object corresponds to zero or more detections. Specifically this does not assume detections have been partitioned into frames of mutual exclusion as is common in 2-D assignment problems.
Parameters: - exists_logits (torch.Tensor) – a tensor of shape
[num_objects]
representing per-object factors for existence of each potential object. - assign_logits (torch.Tensor) – a tensor of shape
[num_detections, num_objects]
representing per-edge factors of assignment probability, where each edge denotes that a given detection associates with a single object. - bp_iters (int) – optional number of belief propagation iterations. If
unspecified or
None
an expensive exact algorithm will be used.
Variables: - num_detections (int) – the number of detections
- num_objects (int) – the number of (potentially existing) objects
- exists_dist (pyro.distributions.Bernoulli) – a mean field posterior distribution over object existence.
- assign_dist (pyro.distributions.Categorical) – a mean field posterior
distribution over the object (or None) to which each detection
associates. This has
.event_shape == (num_objects + 1,)
where the final element denotes spurious detection, and.batch_shape == (num_frames, num_detections)
.
- exists_logits (torch.Tensor) – a tensor of shape
-
class
MarginalAssignmentSparse
(num_objects, num_detections, edges, exists_logits, assign_logits, bp_iters)[source]¶ A cheap sparse version of
MarginalAssignment
.Parameters: - num_detections (int) – the number of detections
- num_objects (int) – the number of (potentially existing) objects
- edges (torch.LongTensor) – a
[2, num_edges]
-shaped tensor of (detection, object) index pairs specifying feasible associations. - exists_logits (torch.Tensor) – a tensor of shape
[num_objects]
representing per-object factors for existence of each potential object. - assign_logits (torch.Tensor) – a tensor of shape
[num_edges]
representing per-edge factors of assignment probability, where each edge denotes that a given detection associates with a single object. - bp_iters (int) – optional number of belief propagation iterations. If
unspecified or
None
an expensive exact algorithm will be used.
Variables: - num_detections (int) – the number of detections
- num_objects (int) – the number of (potentially existing) objects
- exists_dist (pyro.distributions.Bernoulli) – a mean field posterior distribution over object existence.
- assign_dist (pyro.distributions.Categorical) – a mean field posterior
distribution over the object (or None) to which each detection
associates. This has
.event_shape == (num_objects + 1,)
where the final element denotes spurious detection, and.batch_shape == (num_frames, num_detections)
.
-
class
MarginalAssignmentPersistent
(exists_logits, assign_logits, bp_iters=None, bp_momentum=0.5)[source]¶ This computes marginal distributions of a multi-frame multi-object data association problem with an unknown number of persistent objects.
The inputs are factors in a factor graph (existence probabilites for each potential object and assignment probabilities for each object-detection pair), and the outputs are marginal distributions of posterior existence probability of each potential object and posterior assignment probabilites of each object-detection pair.
This assumes a shared (maximum) number of detections per frame; to handle variable number of detections, simply set corresponding elements of
assign_logits
to-float('inf')
.Parameters: - exists_logits (torch.Tensor) – a tensor of shape
[num_objects]
representing per-object factors for existence of each potential object. - assign_logits (torch.Tensor) – a tensor of shape
[num_frames, num_detections, num_objects]
representing per-edge factors of assignment probability, where each edge denotes that at a given time frame a given detection associates with a single object. - bp_iters (int) – optional number of belief propagation iterations. If
unspecified or
None
an expensive exact algorithm will be used. - bp_momentum (float) – optional momentum to use for belief propagation.
Should be in the interval
[0,1)
.
Variables: - num_frames (int) – the number of time frames
- num_detections (int) – the (maximum) number of detections per frame
- num_objects (int) – the number of (potentially existing) objects
- exists_dist (pyro.distributions.Bernoulli) – a mean field posterior distribution over object existence.
- assign_dist (pyro.distributions.Categorical) – a mean field posterior
distribution over the object (or None) to which each detection
associates. This has
.event_shape == (num_objects + 1,)
where the final element denotes spurious detection, and.batch_shape == (num_frames, num_detections)
.
- exists_logits (torch.Tensor) – a tensor of shape
-
compute_marginals
(exists_logits, assign_logits)[source]¶ This implements exact inference of pairwise marginals via enumeration. This is very expensive and is only useful for testing.
See
MarginalAssignment
for args and problem description.
-
compute_marginals_bp
(exists_logits, assign_logits, bp_iters)[source]¶ This implements approximate inference of pairwise marginals via loopy belief propagation, adapting the approach of [1].
See
MarginalAssignment
for args and problem description.- [1] Jason L. Williams, Roslyn A. Lau (2014)
- Approximate evaluation of marginal association probabilities with belief propagation https://arxiv.org/abs/1209.6299
-
compute_marginals_sparse_bp
(num_objects, num_detections, edges, exists_logits, assign_logits, bp_iters)[source]¶ This implements approximate inference of pairwise marginals via loopy belief propagation, adapting the approach of [1].
See
MarginalAssignmentSparse
for args and problem description.- [1] Jason L. Williams, Roslyn A. Lau (2014)
- Approximate evaluation of marginal association probabilities with belief propagation https://arxiv.org/abs/1209.6299
-
compute_marginals_persistent
(exists_logits, assign_logits)[source]¶ This implements exact inference of pairwise marginals via enumeration. This is very expensive and is only useful for testing.
See
MarginalAssignmentPersistent
for args and problem description.
-
compute_marginals_persistent_bp
(exists_logits, assign_logits, bp_iters, bp_momentum=0.5)[source]¶ This implements approximate inference of pairwise marginals via loopy belief propagation, adapting the approach of [1], [2].
See
MarginalAssignmentPersistent
for args and problem description.- [1] Jason L. Williams, Roslyn A. Lau (2014)
- Approximate evaluation of marginal association probabilities with belief propagation https://arxiv.org/abs/1209.6299
- [2] Ryan Turner, Steven Bottone, Bhargav Avasarala (2014)
- A Complete Variational Tracker https://papers.nips.cc/paper/5572-a-complete-variational-tracker.pdf
Distributions¶
-
class
EKFDistribution
(x0, P0, dynamic_model, measurement_cov, time_steps=1, dt=1.0, validate_args=None)[source]¶ Distribution over EKF states. See
EKFState
. Currently only supports log_prob.Parameters: - x0 (torch.Tensor) – PV tensor (mean)
- P0 (torch.Tensor) – covariance
- dynamic_model –
DynamicModel
object - measurement_cov (torch.Tensor) – measurement covariance
- time_steps (int) – number time step
- dt (torch.Tensor) – time step
-
filter_states
(value)[source]¶ Returns the ekf states given measurements
Parameters: value (torch.Tensor) – measurement means of shape (time_steps, event_shape)
-
log_prob
(value)[source]¶ Returns the joint log probability of the innovations of a tensor of measurements
Parameters: value (torch.Tensor) – measurement means of shape (time_steps, event_shape)
Dynamic Models¶
-
class
DynamicModel
(dimension, dimension_pv, num_process_noise_parameters=None)[source]¶ Dynamic model interface.
Parameters: - dimension – native state dimension.
- dimension_pv – PV state dimension.
- num_process_noise_parameters – process noise parameter space dimension.
This for UKF applications. Can be left as
None
for EKF and most other filters.
-
dimension
¶ Native state dimension access.
-
dimension_pv
¶ PV state dimension access.
-
num_process_noise_parameters
¶ Process noise parameters space dimension access.
-
forward
(x, dt, do_normalization=True)[source]¶ Integrate native state
x
over time intervaldt
.Parameters: - x – current native state. If the DynamicModel is non-differentiable,
be sure to handle the case of
x
being augmented with process noise parameters. - dt – time interval to integrate over.
- do_normalization – whether to perform normalization on output, e.g., mod’ing angles into an interval.
Returns: Native state x integrated dt into the future.
- x – current native state. If the DynamicModel is non-differentiable,
be sure to handle the case of
-
geodesic_difference
(x1, x0)[source]¶ Compute and return the geodesic difference between 2 native states. This is a generalization of the Euclidean operation
x1 - x0
.Parameters: - x1 – native state.
- x0 – native state.
Returns: Geodesic difference between native states
x1
andx2
.
-
mean2pv
(x)[source]¶ Compute and return PV state from native state. Useful for combining state estimates of different types in IMM (Interacting Multiple Model) filtering.
Parameters: x – native state estimate mean. Returns: PV state estimate mean.
-
cov2pv
(P)[source]¶ Compute and return PV covariance from native covariance. Useful for combining state estimates of different types in IMM (Interacting Multiple Model) filtering.
Parameters: P – native state estimate covariance. Returns: PV state estimate covariance.
-
process_noise_cov
(dt=0.0)[source]¶ Compute and return process noise covariance (Q).
Parameters: dt – time interval to integrate over. Returns: Read-only covariance (Q). For a DifferentiableDynamicModel, this is the covariance of the native state x
resulting from stochastic integration (for use with EKF). Otherwise, it is the covariance directly of the process noise parameters (for use with UKF).
-
class
DifferentiableDynamicModel
(dimension, dimension_pv, num_process_noise_parameters=None)[source]¶ DynamicModel for which state transition Jacobians can be efficiently calculated, usu. analytically or by automatic differentiation.
-
class
Ncp
(dimension, sv2)[source]¶ NCP (Nearly-Constant Position) dynamic model. May be subclassed, e.g., with CWNV (Continuous White Noise Velocity) or DWNV (Discrete White Noise Velocity).
Parameters: - dimension – native state dimension.
- sv2 – variance of velocity. Usually chosen so that the standard deviation is roughly half of the max velocity one would ever expect to observe.
-
forward
(x, dt, do_normalization=True)[source]¶ Integrate native state
x
over time intervaldt
.Parameters: - x – current native state. If the DynamicModel is non-differentiable,
be sure to handle the case of
x
being augmented with process noise parameters. - dt – time interval to integrate over. do_normalization: whether to perform normalization on output, e.g., mod’ing angles into an interval. Has no effect for this subclass.
Returns: Native state x integrated dt into the future.
- x – current native state. If the DynamicModel is non-differentiable,
be sure to handle the case of
-
mean2pv
(x)[source]¶ Compute and return PV state from native state. Useful for combining state estimates of different types in IMM (Interacting Multiple Model) filtering.
Parameters: x – native state estimate mean. Returns: PV state estimate mean.
-
cov2pv
(P)[source]¶ Compute and return PV covariance from native covariance. Useful for combining state estimates of different types in IMM (Interacting Multiple Model) filtering.
Parameters: P – native state estimate covariance. Returns: PV state estimate covariance.
-
class
Ncv
(dimension, sa2)[source]¶ NCV (Nearly-Constant Velocity) dynamic model. May be subclassed, e.g., with CWNA (Continuous White Noise Acceleration) or DWNA (Discrete White Noise Acceleration).
Parameters: - dimension – native state dimension.
- sa2 – variance of acceleration. Usually chosen so that the standard deviation is roughly half of the max acceleration one would ever expect to observe.
-
forward
(x, dt, do_normalization=True)[source]¶ Integrate native state
x
over time intervaldt
.Parameters: - x – current native state. If the DynamicModel is non-differentiable,
be sure to handle the case of
x
being augmented with process noise parameters. - dt – time interval to integrate over.
- do_normalization – whether to perform normalization on output, e.g., mod’ing angles into an interval. Has no effect for this subclass.
Returns: Native state x integrated dt into the future.
- x – current native state. If the DynamicModel is non-differentiable,
be sure to handle the case of
-
mean2pv
(x)[source]¶ Compute and return PV state from native state. Useful for combining state estimates of different types in IMM (Interacting Multiple Model) filtering.
Parameters: x – native state estimate mean. Returns: PV state estimate mean.
-
cov2pv
(P)[source]¶ Compute and return PV covariance from native covariance. Useful for combining state estimates of different types in IMM (Interacting Multiple Model) filtering.
Parameters: P – native state estimate covariance. Returns: PV state estimate covariance.
-
class
NcpContinuous
(dimension, sv2)[source]¶ NCP (Nearly-Constant Position) dynamic model with CWNV (Continuous White Noise Velocity).
- References:
- “Estimation with Applications to Tracking and Navigation” by Y. Bar- Shalom et al, 2001, p.269.
Parameters: - dimension – native state dimension.
- sv2 – variance of velocity. Usually chosen so that the standard deviation is roughly half of the max velocity one would ever expect to observe.
-
class
NcvContinuous
(dimension, sa2)[source]¶ NCV (Nearly-Constant Velocity) dynamic model with CWNA (Continuous White Noise Acceleration).
- References:
- “Estimation with Applications to Tracking and Navigation” by Y. Bar- Shalom et al, 2001, p.269.
Parameters: - dimension – native state dimension.
- sa2 – variance of acceleration. Usually chosen so that the standard deviation is roughly half of the max acceleration one would ever expect to observe.
-
class
NcpDiscrete
(dimension, sv2)[source]¶ NCP (Nearly-Constant Position) dynamic model with DWNV (Discrete White Noise Velocity).
Parameters: - dimension – native state dimension.
- sv2 – variance of velocity. Usually chosen so that the standard deviation is roughly half of the max velocity one would ever expect to observe.
- References:
- “Estimation with Applications to Tracking and Navigation” by Y. Bar- Shalom et al, 2001, p.273.
-
class
NcvDiscrete
(dimension, sa2)[source]¶ NCV (Nearly-Constant Velocity) dynamic model with DWNA (Discrete White Noise Acceleration).
Parameters: - dimension – native state dimension.
- sa2 – variance of acceleration. Usually chosen so that the standard deviation is roughly half of the max acceleration one would ever expect to observe.
- References:
- “Estimation with Applications to Tracking and Navigation” by Y. Bar- Shalom et al, 2001, p.273.
-
process_noise_cov
(dt=0.0)[source]¶ Compute and return cached process noise covariance (Q).
Parameters: dt – time interval to integrate over. Returns: Read-only covariance (Q) of the native state x resulting from stochastic integration (for use with EKF). (Note that this Q, modulo numerical error, has rank dimension/2. So, it is only positive semi-definite.)
Extended Kalman Filter¶
-
class
EKFState
(dynamic_model, mean, cov, time=None, frame_num=None)[source]¶ State-Centric EKF (Extended Kalman Filter) for use with either an NCP (Nearly-Constant Position) or NCV (Nearly-Constant Velocity) target dynamic model. Stores a target dynamic model, state estimate, and state time. Incoming
Measurement
provide sensor information for updates.Warning
For efficiency, the dynamic model is only shallow-copied. Make a deep copy outside as necessary to protect against unexpected changes.
Parameters: - dynamic_model – target dynamic model.
- mean – mean of target state estimate.
- cov – covariance of target state estimate.
- time – time of state estimate.
-
dynamic_model
¶ Dynamic model access.
-
dimension
¶ Native state dimension access.
-
mean
¶ Native state estimate mean access.
-
cov
¶ Native state estimate covariance access.
-
dimension_pv
¶ PV state dimension access.
-
time
¶ Continuous State time access.
-
frame_num
¶ Discrete State time access.
-
predict
(dt=None, destination_time=None, destination_frame_num=None)[source]¶ Use dynamic model to predict (aka propagate aka integrate) state estimate in-place.
Parameters: - dt – time to integrate over. The state time will be automatically
incremented this amount unless you provide
destination_time
. Usingdestination_time
may be preferable for prevention of roundoff error accumulation. - destination_time – optional value to set continuous state time to after integration. If this is not provided, then destination_frame_num must be.
- destination_frame_num – optional value to set discrete state time to after integration. If this is not provided, then destination_frame_num must be.
- dt – time to integrate over. The state time will be automatically
incremented this amount unless you provide
-
innovation
(measurement)[source]¶ Compute and return the innovation that a measurement would induce if it were used for an update, but don’t actually perform the update. Assumes state and measurement are time-aligned. Useful for computing Chi^2 stats and likelihoods.
Parameters: measurement – measurement Returns: Innovation mean and covariance of hypothetical update. Return type: tuple( torch.Tensor
,torch.Tensor
)
-
log_likelihood_of_update
(measurement)[source]¶ Compute and return the likelihood of a potential update, but don’t actually perform the update. Assumes state and measurement are time- aligned. Useful for gating and calculating costs in assignment problems for data association.
Param: measurement. Returns: Likelihood of hypothetical update.
-
update
(measurement)[source]¶ Use measurement to update state estimate in-place and return innovation. The innovation is useful, e.g., for evaluating filter consistency or updating model likelihoods when the
EKFState
is part of anIMMFState
.Param: measurement. Returns: EKF State, Innovation mean and covariance.
Hashing¶
-
class
LSH
(radius)[source]¶ Implements locality-sensitive hashing for low-dimensional euclidean space.
Allows to efficiently find neighbours of a point. Provides 2 guarantees:
- Difference between coordinates of points not returned by
nearby()
and input point is larger thanradius
. - Difference between coordinates of points returned by
nearby()
and input point is smaller than 2radius
.
Example:
>>> radius = 1 >>> lsh = LSH(radius) >>> a = torch.tensor([-0.51, -0.51]) # hash(a)=(-1,-1) >>> b = torch.tensor([-0.49, -0.49]) # hash(a)=(0,0) >>> c = torch.tensor([1.0, 1.0]) # hash(b)=(1,1) >>> lsh.add('a', a) >>> lsh.add('b', b) >>> lsh.add('c', c) >>> # even though c is within 2radius of a >>> lsh.nearby('a') # doctest: +SKIP {'b'} >>> lsh.nearby('b') # doctest: +SKIP {'a', 'c'} >>> lsh.remove('b') >>> lsh.nearby('a') # doctest: +SKIP set()
Parameters: radius (float) – Scaling parameter used in hash function. Determines the size of the neighbourhood. -
add
(key, point)[source]¶ Adds (
key
,point
) pair to the hash.Parameters: - key – Key used identify
point
. - point (torch.Tensor) – data, should be detached and on cpu.
- key – Key used identify
-
remove
(key)[source]¶ Removes
key
and corresponding point from the hash.Raises
KeyError
if key is not in hash.Parameters: key – key used to identify point.
-
nearby
(key)[source]¶ Returns a set of keys which are neighbours of the point identified by
key
.Two points are nearby if difference of each element of their hashes is smaller than 2. In euclidean space, this corresponds to all points \(\mathbf{p}\) where \(|\mathbf{p}_k-(\mathbf{p_{key}})_k|<r\), and some points (all points not guaranteed) where \(|\mathbf{p}_k-(\mathbf{p_{key}})_k|<2r\).
Parameters: key – key used to identify input point. Returns: a set of keys identifying neighbours of the input point. Return type: set
- Difference between coordinates of points not returned by
-
class
ApproxSet
(radius)[source]¶ Queries low-dimensional euclidean space for approximate occupancy.
Parameters: radius (float) – scaling parameter used in hash function. Determines the size of the bin. See LSH
for details.-
try_add
(point)[source]¶ Attempts to add
point
to set. Only adds there are no points in thepoint
’s bin.Parameters: point (torch.Tensor) – Point to be queried, should be detached and on cpu. Returns: True
if point is successfully added,False
if there is already a point inpoint
’s bin.Return type: bool
-
-
merge_points
(points, radius)[source]¶ Greedily merge points that are closer than given radius.
This uses
LSH
to achieve complexity that is linear in the number of merged clusters and quadratic in the size of the largest merged cluster.Parameters: - points (torch.Tensor) – A tensor of shape
(K,D)
whereK
is the number of points andD
is the number of dimensions. - radius (float) – The minimum distance nearer than which points will be merged.
Returns: A tuple
(merged_points, groups)
wheremerged_points
is a tensor of shape(J,D)
whereJ <= K
, andgroups
is a list of tuples of indices mapping merged points to original points. Note thatlen(groups) == J
andsum(len(group) for group in groups) == K
.Return type: - points (torch.Tensor) – A tensor of shape
Measurements¶
-
class
Measurement
(mean, cov, time=None, frame_num=None)[source]¶ Gaussian measurement interface.
Parameters: - mean – mean of measurement distribution.
- cov – covariance of measurement distribution.
- time – continuous time of measurement. If this is not provided, frame_num must be.
- frame_num – discrete time of measurement. If this is not provided, time must be.
-
dimension
¶ Measurement space dimension access.
-
mean
¶ Measurement mean (
z
in most Kalman Filtering literature).
-
cov
¶ Noise covariance (
R
in most Kalman Filtering literature).
-
time
¶ Continuous time of measurement.
-
frame_num
¶ Discrete time of measurement.
-
class
DifferentiableMeasurement
(mean, cov, time=None, frame_num=None)[source]¶ Interface for Gaussian measurement for which Jacobians can be efficiently calculated, usu. analytically or by automatic differentiation.