d3d.tracking

This module contains implementation of tracking algorithms and utilities.

d3d.tracking.tracker

class d3d.tracking.tracker.VanillaTracker(pose_tracker_factory=<class 'd3d.tracking.filter.Pose_3DOF_UKF_CTRA'>, feature_tracker_factory=<class 'd3d.tracking.filter.Box_KF'>, matcher_factory=<class 'd3d.tracking.matcher.HungarianMatcher'>, matcher_distance_type='position', matcher_distance_threshold=1, lost_time=1, default_position_var=array([[1., 0., 0.],        [0., 1., 0.],        [0., 0., 1.]]), default_dimension_var=array([[1., 0., 0.],        [0., 1., 0.],        [0., 0., 1.]]), default_orientation_var=1)[source]

Bases: object

Implementation of a vanilla tracker based on Kalman Filter

Parameters

  • lost_time – determine the time length of a target being lost before it’s removed from tracking

  • pose_tracker_factory – factory function to generate a new pose tracker, takes only initial detection as input

  • feature_tracker_factory – factory function to generate a new feature tracker, takes only initial detection as input

  • matcher_factory – factory function to generate a new target matcher

  • matcher_distance_type – distance type used to match targets

  • matcher_distance_threshold – distance threshold used in target matcher

  • default_position_var – default positional covariance assigned to targets (if not provided)

  • default_dimension_var – default dimensional covariance assigned to targets (if not provided)

  • default_orientation_var – default orientational covariance assigned to targets (if not provided)

report()[source]

Return the collection of valid tracked targets

Return type

d3d.abstraction.Target3DArray

property tracked_ids

Return a ID list of actively tracked targets

update(detections)[source]

Update the filters when receiving new detections

Parameters

detections (d3d.abstraction.Target3DArray) –

d3d.tracking.matcher

class d3d.tracking.matcher.BaseMatcher

Bases: object

This class is the base class of various matchers

clear_match(self) → void

Clear saved match results

match(self, vector[int] src_subset, vector[int] dst_subset, unordered_map[int, float] distance_threshold) → void
Parameters

  • src_subset – Indices of source boxes to be considered

  • dst_subset – Indices of destination boxes to be considered

  • distance_threshold – threshold of maximum distance for each category. The category should be represented as its value.

num_of_matches(self) → int
prepare_boxes(self, Target3DArray src_boxes, Target3DArray dst_boxes, DistanceTypes distance_metric) → void

This method add two arrays of boxes and prepare related informations, it will also clean previous results.

Parameters

  • src_boxes – boxes to match

  • dst_boxes – fixed boxes (such as ground truth boxes)

  • distance_metric – the metric for calculating the “distance”

query_dst_match(self, int dst_idx) → int
query_src_match(self, int src_idx) → int
class d3d.tracking.matcher.HungarianMatcher

Bases: d3d.tracking.matcher.BaseMatcher

This matcher select target correspondences using the Hungarian Algorithm

match(self, vector[int] src_subset, vector[int] dst_subset, unordered_map[int, float] distance_threshold) → void
class d3d.tracking.matcher.NearestNeighborMatcher

Bases: d3d.tracking.matcher.BaseMatcher

This matcher select target correspondences from closest pair to farthest pair

match(self, vector[int] src_subset, vector[int] dst_subset, unordered_map[int, float] distance_threshold) → void
class d3d.tracking.matcher.ScoreMatcher

Bases: d3d.tracking.matcher.BaseMatcher

This matcher select target correspondences from highest score to lowest score

match(self, vector[int] src_subset, vector[int] dst_subset, unordered_map[int, float] distance_threshold) → void

d3d.tracking.filter

class d3d.tracking.filter.Box_KF(init, Q=array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]))[source]

Bases: d3d.tracking.filter.PropertyFilter

Use kalman filter (simple bayesian filter) for a box shape estimation. Use latest value for classification result

Parameters
property classification

Current classification estimation

property classification_var

Covariance for current classification estimation

property dimension

Current dimension estimation

property dimension_var

Covariance for current shape estimation

predict(dt)[source]

Predict the current system state

Parameters

dt – Time since last update

update(target)[source]

Correct the current system state with observation. This method should always be called after calling predict().

Parameters

target – New target observation

class d3d.tracking.filter.PoseFilter[source]

Bases: object

Abstraction class defining the interfaces for filters on target pose

property angular_velocity: numpy.ndarray

Current angular velocity estimation

property angular_velocity_var: numpy.ndarray

Covariance for current angular velocity esimation

property orientation: numpy.ndarray

Current orientation estimation

property orientation_var: numpy.ndarray

Covariance for current orientation estimation

property position: numpy.ndarray

Current position estimation

property position_var: numpy.ndarray

Covariance for current position estimation

predict(dt)[source]

Predict the current system state

Parameters

dt (float) – Time since last update

update(target)[source]

Correct the current system state with observation. This method should always be called after calling predict().

Parameters

target (Union[d3d.abstraction.ObjectTarget3D, d3d.abstraction.TrackingTarget3D]) – New target observation

property velocity: numpy.ndarray

Current linear velocity estimation

property velocity_var: numpy.ndarray

Covariance for current linear velocity estimation

class d3d.tracking.filter.Pose_3DOF_UKF_CTRA(init, Q=array([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 1.0]]))[source]

Bases: d3d.tracking.filter.PoseFilter

UKF using constant turning rate and acceleration (CTRA) model for pose estimation

States: ..

[x, y, rz, v, a, w] [0 1 2 3 4 5]

Observe: ..

[x, y, rz] [0 1 2 ]

Parameters
property angular_velocity

Current angular velocity estimation

property angular_velocity_var

Covariance for current angular velocity esimation

property orientation

Current orientation estimation

property orientation_var

Covariance for current orientation estimation

property position

Current position estimation

property position_var

Covariance for current position estimation

predict(dt)[source]

Predict the current system state

Parameters

dt – Time since last update

update(detection)[source]

Correct the current system state with observation. This method should always be called after calling predict().

Parameters
property velocity

Current linear velocity estimation

property velocity_var

Covariance for current linear velocity estimation

class d3d.tracking.filter.Pose_3DOF_UKF_CTRV[source]

Bases: d3d.tracking.filter.PoseFilter

UKF using constant turning rate and velocity (CTRV) model for pose estimation

States: ..

[x, y, rz, v, w] [0 1 2 3 4]

Observe: ..

[x, y, rz] [0 1 2 ]

class d3d.tracking.filter.Pose_3DOF_UKF_CV(init, Q=array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]))[source]

Bases: d3d.tracking.filter.PoseFilter

UKF using constant velocity (CV) model for pose estimation, assuming 3DoF (x, y, yaw)

States: ..

[x, y, vx, vy] [0 1 2 3 ]

Observe: ..

[x, y] [0 1]

Parameters
property angular_velocity

Current angular velocity estimation

property angular_velocity_var

Covariance for current angular velocity esimation

property orientation

Current orientation estimation

property orientation_var

Covariance for current orientation estimation

property position

Current position estimation

property position_var

Covariance for current position estimation

predict(dt)[source]

Predict the current system state

Parameters

dt – Time since last update

update(detection)[source]

Correct the current system state with observation. This method should always be called after calling predict().

Parameters
property velocity

Current linear velocity estimation

property velocity_var

Covariance for current linear velocity estimation

class d3d.tracking.filter.Pose_IMM[source]

Bases: d3d.tracking.filter.PoseFilter

UKF using IMM model for pose estimation

class d3d.tracking.filter.PropertyFilter[source]

Bases: object

Abstraction class defining the interfaces for filters on target property

property classification: numpy.ndarray

Current classification estimation

property classification_var: numpy.ndarray

Covariance for current classification estimation

property dimension: numpy.ndarray

Current dimension estimation

property dimension_var: numpy.ndarray

Covariance for current shape estimation

predict(dt)[source]

Predict the current system state

Parameters

dt (float) – Time since last update

update(target)[source]

Correct the current system state with observation. This method should always be called after calling predict().

Parameters

target (Union[d3d.abstraction.ObjectTarget3D, d3d.abstraction.TrackingTarget3D]) – New target observation

d3d.tracking.filter.is_pd(B)[source]

Returns true when input is positive-definite, via Cholesky

d3d.tracking.filter.motion_CSAA(state, dt)[source]

Constant Steering Angle and Acceleration model.

Parameters

  • state (numpy.ndarray) – original state in format [x, y, theta, v, a, c] [0 1 2 3 4 5]

  • dt (float) – time difference after last update

Returns

updated state

Return type

numpy.ndarray

d3d.tracking.filter.motion_CTRA(state, dt)[source]

Constant Turn-Rate and (longitudinal) Acceleration model. This model also assume that velocity is the same with heading angle. CV, CTRV can be modeled by assume value equals zero

Parameters
Returns

updated state

Return type

numpy.ndarray

d3d.tracking.filter.motion_CV(state, dt)[source]

Constant Velocity model

Parameters
Returns

updated state

Return type

numpy.ndarray

d3d.tracking.filter.nearest_pd(A)[source]

Find the nearest positive-definite matrix to input A Python/Numpy port of John D’Errico’s nearestSPD MATLAB code [1], which credits [2].

d3d.tracking.filter.wrap_angle(theta)[source]

Normalize the angle to [-pi, pi)

Parameters

theta (float) – angle to be wrapped

Returns

wrapped angle

Return type

float