diff --git a/stonesoup/dataassociator/_assignment.py b/stonesoup/dataassociator/_assignment.py deleted file mode 100644 index 75d285a45..000000000 --- a/stonesoup/dataassociator/_assignment.py +++ /dev/null @@ -1,257 +0,0 @@ -import numpy as np - - -def assign2D(C, maximize=False): - # ASSIGN2D: - # Solve the two-dimensional assignment problem with a rectangular - # cost matrix C, scanning row-wise. The problem being solved can - # be formulated as minimize (or maximize): - # \sum_{i=1}^{numRow}\sum_{j=1}^{numCol}C_{i,j}*x_{i,j} - # subject to: - # \sum_{j=1}^{numCol}x_{i,j}<=1 for all i - # \sum_{i=1}^{numRow}x_{i,j}=1 for all j - # x_{i,j}=0 or 1. - # Assuming that numCol<=numRow. If numCol>numRow, then the - # inequality and inequality conditions are switched. A modified - # Jonker-Volgenant algorithm is used. - # - # INPUTS: - # C A numRowXnumCol 2D numpy array or matrix matrix that does - # not contain any NaNs. Forbidden assignments can be given costs - # of +Inf for minimization and -Inf for maximization. - # maximize A boolean value. If true, the minimization problem is - # transformed into a maximization problem. The default - # if this parameter is omitted or an empty matrix - # is passed is false. - # - # OUTPUTS: - # gain The sum of the values of the assigned elements in C. If the - # problem is infeasible, this is `None`. - # col4row A length numRow numpy array where the entry in each - # element is an assignment of the element in that row to - # a column. 0 entries signify unassigned rows. If the - # problem is infeasible, this is an empty matrix. - # row4col A length numCol numpy array where the entry in each - # element is an assignment of the element in that column - # to a row. 0 entries signify unassigned columns. If the - # problem is infeasible, this is an empty matrix. - # - # If the number of rows is <= the number of columns, then every row is - # assigned to one column; otherwise every column is assigned to one - # row. The assignment minimizes the sum of the assigned elements (the - # gain). During minimization, assignments can be forbidden by placing - # Inf in elements. During maximization, assignment can be forbidden by - # placing -Inf in elements. The cost matrix can not contain any -Inf - # elements during minimization nor any +Inf elements during - # maximization to try to force an assignment. If no complete - # assignment can be made with finite cost, then gain, col4row, and - # row4col are returned as numpy.empty(0) values. - # - # The algorithm is described in detail in [1] and [2]. This is a Python - # translation of the C and Matlab functions in the - # Tracker Component Library of - # https://github.com/USNavalResearchLaboratory/TrackerComponentLibrary - # - # EXAMPLE 1: - # import numpy - # import assignAlgs - # Inf=numpy.inf - # C=numpy.array([[Inf, 2, Inf, Inf, 3], - # [ 7, Inf, 23, Inf, Inf], - # [ 17, 24, Inf, Inf, Inf], - # [ Inf, 6, 13, 20, Inf]]) - # maximize=False - # gain, col4row, row4col=assignAlgs.assign2D(C,maximize) - # print(gain) - # One will get an optimal assignment having a gain of 47 - # - # EXAMPLE 2: - # This is the example used in [3]. Here, we demonstrate how to form - # assignment tuples (index from 1, not 0) from col4row. - # import numpy - # import assignAlgs - # C=numpy.array([[7, 51, 52, 87, 38, 60, 74, 66, 0, 20], - # [50, 12, 0, 64, 8, 53, 0, 46, 76, 42], - # [27, 77, 0, 18, 22, 48, 44, 13, 0, 57], - # [62, 0, 3, 8, 5, 6, 14, 0, 26, 39], - # [0, 97, 0, 5, 13, 0, 41, 31, 62, 48], - # [79, 68, 0, 0, 15, 12, 17, 47, 35, 43], - # [76, 99, 48, 27, 34, 0, 0, 0, 28, 0], - # [0, 20, 9, 27, 46, 15, 84, 19, 3, 24], - # [56, 10, 45, 39, 0, 93, 67, 79, 19, 38], - # [27, 0, 39, 53, 46, 24, 69, 46, 23, 1]]) - # maximize=False - # gain, col4row, row4col=assignAlgs.assign2D(C,maximize) - # tuples=numpy.empty((2,10),dtype=int) - # for curRow in range(0,10): - # tuples[0,curRow]=curRow+1 - # tuples[1,curRow]=col4row[curRow]+1 - # print(gain) - # print(tuples) - # One will see that the gain is 0 and the assigned tuples match what - # is in [3]. However, the assigned tuples is NOT obtained by attaching - # col4row to row4col. - # - # REFERENCES: - # [1] D. F. Crouse, "On Implementing 2D Rectangular Assignment - # Algorithms," IEEE Transactions on Aerospace and Electronic - # Systems, vol. 52, no. 4, pp. 1679-1696, Aug. 2016. - # [2] D. F. Crouse, "Advances in displaying uncertain estimates of - # multiple targets," in Proceedings of SPIE: Signal Processing, - # Sensor Fusion, and Target Recognition XXII, vol. 8745, - # Baltimore, MD, Apr. 2013. - # [3] Murty, K. G. "An algorithm for ranking all the assignments in - # order of increasing cost," Operations Research, vol. 16, no. 3, - # pp. 682-687, May-Jun. 1968. - # - # May 2018 David F. Crouse, Naval Research Laboratory, Washington D.C. - # (UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release. - # This work was supported by the Office of Naval Research through the - # Naval Research Laboratory 6.1 Base Program - - numRow, numCol = C.shape - didFlip = False - - if numCol > numRow: - C = C.T - numRow, numCol = numCol, numRow - didFlip = True - - # The cost matrix must have all non-negative elements for the - # assignment algorithm to work. This forces all of the elements to be - # positive. The delta is added back in when computing the gain in the - # end. - if not maximize: - CDelta = np.min(C) - # If C is all positive, do not shift. - if CDelta > 0: - CDelta = 0 - else: - C = C - CDelta - else: - CDelta = np.max(C) - # If C is all negative, do not shift. - if CDelta < 0: - CDelta = 0 - C = -C + CDelta - - gain, col4row, row4col = assign2DBasic(C) - - if gain is None: - # The problem is infeasible - emptyMat = np.empty(0) - return gain, emptyMat, emptyMat - - # The problem is feasible. Adjust for the shifting of the elements - # in C. - if not maximize: - gain = gain + CDelta*numCol - else: - gain = -gain + CDelta*numCol - - # If a transposed matrix was used - if didFlip: - col4row, row4col = row4col, col4row - - return gain, col4row, row4col - - -def assign2DBasic(C): - numRow, numCol = C.shape - - col4row = np.full(numRow, -1, dtype=np.intp) - row4col = np.full(numCol, -1, dtype=np.intp) - u = np.zeros(numCol) - v = np.zeros(numRow) - - # pred will be used to keep track of the shortest path. - pred = np.empty(numRow, dtype=np.intp) - - for curUnassignedCol in range(numCol): - # First, find the shortest augmenting path starting at - # curUnassignedCol. - - # Mark everything as not yet scanned. - scannedCols = np.zeros(numCol, dtype=bool) - scannedRows = np.zeros(numRow, dtype=bool) - # Initially, the cost of the shortest path to each row is not - # known and will be made infinite. - shortestPathCost = np.full(numRow, np.inf) - - # sink will hold the final index of the shortest augmenting path. - # If the problem is not feasible, then sink will remain -1. - sink = -1 - delta = 0. - curCol = curUnassignedCol - - while sink == -1: - # Mark the current column as having been visited. - scannedCols[curCol] = True - - reducedCosts = delta + C[~scannedRows, curCol] - u[curCol] - v[~scannedRows] - - idx = reducedCosts < shortestPathCost[~scannedRows] - idx_orig = np.flatnonzero(~scannedRows)[idx] - pred[idx_orig] = curCol - shortestPathCost[idx_orig] = reducedCosts[idx] - - argmin = np.argmin(shortestPathCost[~scannedRows]) - closestRow = np.flatnonzero(~scannedRows)[argmin] - delta = shortestPathCost[closestRow] - - if delta == np.inf: - # If the minimum cost row is not finite, then the - # problem is not feasible. - return None, col4row, row4col - - # Add the closest row to the list of scanned rows - scannedRows[closestRow] = True - - # If we have reached an unassigned column - if col4row[closestRow] == -1: - sink = closestRow - else: - curCol = col4row[closestRow] - - # Next, update the dual variables. - # Update the first column in the augmenting path. - u[curUnassignedCol] += delta - - # Update the rest of the columns in the augmenting path. - # Skipping curUnassignedCol. - scannedCols[curUnassignedCol] = False - u[scannedCols] += delta - shortestPathCost[row4col[scannedCols]] - - # Update the rows in the augmenting path. - v[scannedRows] += shortestPathCost[scannedRows] - delta - - # Remove the current node from those that must be assigned. - curRow = sink - curCol = -1 - while curCol != curUnassignedCol: - curCol = pred[curRow] - col4row[curRow] = curCol - row4col[curCol], curRow = curRow, row4col[curCol] - - # Determine the gain to return - gain = sum(C[row4col[curCol], curCol] for curCol in range(numCol)) - - return gain, col4row, row4col - -# LICENSE: -# -# The source code is in the public domain and not licensed or under -# copyright. The information and software may be used freely by the public. -# As required by 17 U.S.C. 403, third parties producing copyrighted works -# consisting predominantly of the material produced by U.S. government -# agencies must provide notice with such work(s) identifying the U.S. -# Government material incorporated and stating that such material is not -# subject to copyright protection. -# -# Derived works shall not identify themselves in a manner that implies an -# endorsement by or an affiliation with the Naval Research Laboratory. -# -# RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE -# SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL -# RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS -# OF RECIPIENT IN THE USE OF THE SOFTWARE. diff --git a/stonesoup/dataassociator/neighbour.py b/stonesoup/dataassociator/neighbour.py index 6a07dad6b..2ea26f6da 100644 --- a/stonesoup/dataassociator/neighbour.py +++ b/stonesoup/dataassociator/neighbour.py @@ -1,8 +1,8 @@ import itertools import numpy as np +from scipy.optimize import linear_sum_assignment -from ._assignment import assign2D from .base import DataAssociator from ..base import Property from ..hypothesiser import Hypothesiser @@ -234,11 +234,9 @@ def associate(self, tracks, detections, timestamp, **kwargs): # to assign tracks to nearest detection # Maximise flag = true for probability instance # (converts minimisation problem to maximisation problem) - gain, col4row, row4col = assign2D( - distance_matrix, probability_flag) - - # Ensure the problem was feasible - if gain is None: + try: + row4col, col4row = linear_sum_assignment(distance_matrix, probability_flag) + except ValueError: raise RuntimeError("Assignment was not feasible") # Generate dict of key/value pairs