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
x0 (torch.Tensor) – PV tensor (mean)
P0 (torch.Tensor) – covariance
dynamic_model –
DynamicModel
objectmeasurement_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.
- property dimension¶
Native state dimension access.
- property dimension_pv¶
PV state dimension access.
- property num_process_noise_parameters¶
Process noise parameters space dimension access.
- abstract 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.
- 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
.
- abstract 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.
- abstract 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.
- abstract 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.
- 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.
- 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.
- property dynamic_model¶
Dynamic model access.
- property dimension¶
Native state dimension access.
- property mean¶
Native state estimate mean access.
- property cov¶
Native state estimate covariance access.
- property dimension_pv¶
PV state dimension access.
- property mean_pv¶
Compute and return cached PV state estimate mean.
- property cov_pv¶
Compute and return cached PV state estimate covariance.
- property time¶
Continuous State time access.
- property 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.
- 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') {'b'} >>> lsh.nearby('b') {'a', 'c'} >>> lsh.remove('b') >>> lsh.nearby('a') 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
- 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
- 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
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.
- property dimension¶
Measurement space dimension access.
- property mean¶
Measurement mean (
z
in most Kalman Filtering literature).
- property cov¶
Noise covariance (
R
in most Kalman Filtering literature).
- property time¶
Continuous time of measurement.
- property 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.