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).
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).
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:
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 interval dt.

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.

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 and x2.

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).
process_noise_dist(dt=0.0)[source]

Return a distribution object of state displacement from the process noise distribution over a time interval.

Parameters:dt – time interval that process noise accumulates over.
Returns:MultivariateNormal.
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.

jacobian(dt)[source]

Compute and return native state transition Jacobian (F) over time interval dt.

Parameters:dt – time interval to integrate over.
Returns:Read-only Jacobian (F) of integration map (f).
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 interval dt.

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.

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.
jacobian(dt)[source]

Compute and return cached native state transition Jacobian (F) over time interval dt.

Parameters:dt – time interval to integrate over.
Returns:Read-only Jacobian (F) of integration map (f).
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).
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 interval dt.

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.

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.
jacobian(dt)[source]

Compute and return cached native state transition Jacobian (F) over time interval dt.

Parameters:dt – time interval to integrate over.
Returns:Read-only Jacobian (F) of integration map (f).
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).
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.
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).
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.
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).
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.
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).
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.

mean_pv[source]

Compute and return cached PV state estimate mean.

cov_pv[source]

Compute and return cached PV state estimate covariance.

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. Using destination_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.
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 an IMMFState.

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 than radius.
  • Difference between coordinates of points returned by nearby() and input point is smaller than 2 radius.

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.
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
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 the point’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 in point’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) where K is the number of points and D is the number of dimensions.
  • radius (float) – The minimum distance nearer than which points will be merged.
Returns:

A tuple (merged_points, groups) where merged_points is a tensor of shape (J,D) where J <= K, and groups is a list of tuples of indices mapping merged points to original points. Note that len(groups) == J and sum(len(group) for group in groups) == K.

Return type:

tuple

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.

geodesic_difference(z1, z0)[source]

Compute and return the geodesic difference between 2 measurements. This is a generalization of the Euclidean operation z1 - z0.

Parameters:
  • z1 – measurement.
  • z0 – measurement.
Returns:

Geodesic difference between z1 and z2.

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.

jacobian(x=None)[source]

Compute and return Jacobian (H) of measurement map (h) at target PV state x .

Parameters:x – PV state. Use default argument None when the Jacobian is not state-dependent.
Returns:Read-only Jacobian (H) of measurement map (h).
class PositionMeasurement(mean, cov, time=None, frame_num=None)[source]

Full-rank Gaussian position measurement in Euclidean space.

Parameters:
  • mean – mean of measurement distribution.
  • cov – covariance of measurement distribution.
  • time – time of measurement.
jacobian(x=None)[source]

Compute and return Jacobian (H) of measurement map (h) at target PV state x .

Parameters:x – PV state. The default argument None may be used in this subclass since the Jacobian is not state-dependent.
Returns:Read-only Jacobian (H) of measurement map (h).