From f9a8b8cbedfb82868974f90a2369f7fff5f836dd Mon Sep 17 00:00:00 2001
From: mwinter <mwinter@uwm.edu>
Date: Thu, 7 Oct 2010 23:14:58 -0500
Subject: [PATCH] Single frame assignment tracker with same cost function as
our tracker
Uses munkres rectangular assignment algorithm
---
.gitignore | 5 +-
KymoTracker_v2.vcproj | 20 +-
src/hungarian.c | 419 ++++++++++++++++++++++++++++++++++++++++
src/hungarian.h | 78 ++++++++
src/munkres/matrix.cpp | 231 ++++++++++++++++++++++
src/munkres/matrix.h | 59 ++++++
src/munkres/munkres.cpp | 336 ++++++++++++++++++++++++++++++++
src/munkres/munkres.h | 46 +++++
src/tracker.cpp | 133 +++++++++++--
src/tracker.h | 1 +
10 files changed, 1305 insertions(+), 23 deletions(-)
create mode 100644 src/hungarian.c
create mode 100644 src/hungarian.h
create mode 100644 src/munkres/matrix.cpp
create mode 100644 src/munkres/matrix.h
create mode 100644 src/munkres/munkres.cpp
create mode 100644 src/munkres/munkres.h
diff --git a/.gitignore b/.gitignore
index f3703bc..15493bc 100644
--- a/.gitignore
+++ b/.gitignore
@@ -16,4 +16,7 @@
*.dep
# Ignore matlab autosaves
-*.asv
\ No newline at end of file
+*.asv
+
+# Ignore log output files
+*.log
\ No newline at end of file
diff --git a/KymoTracker_v2.vcproj b/KymoTracker_v2.vcproj
index 6334437..472516d 100644
--- a/KymoTracker_v2.vcproj
+++ b/KymoTracker_v2.vcproj
@@ -41,7 +41,7 @@
<Tool
Name="VCCLCompilerTool"
Optimization="0"
- AdditionalIncludeDirectories="clapack/include"
+ AdditionalIncludeDirectories="clapack/include;src/munkres"
PreprocessorDefinitions="WIN32;_DEBUG;_CONSOLE"
MinimalRebuild="true"
BasicRuntimeChecks="3"
@@ -117,7 +117,7 @@
Name="VCCLCompilerTool"
Optimization="2"
EnableIntrinsicFunctions="true"
- AdditionalIncludeDirectories="clapack/include"
+ AdditionalIncludeDirectories="clapack/include;src/munkres"
PreprocessorDefinitions="WIN32;NDEBUG;_CONSOLE"
RuntimeLibrary="0"
EnableFunctionLevelLinking="true"
@@ -184,6 +184,14 @@
RelativePath=".\src\detection.cpp"
>
</File>
+ <File
+ RelativePath=".\src\hungarian.c"
+ >
+ </File>
+ <File
+ RelativePath=".\src\munkres\munkres.cpp"
+ >
+ </File>
<File
RelativePath=".\src\paths.cpp"
>
@@ -206,6 +214,14 @@
RelativePath=".\src\detection.h"
>
</File>
+ <File
+ RelativePath=".\src\hungarian.h"
+ >
+ </File>
+ <File
+ RelativePath=".\src\munkres\munkres.h"
+ >
+ </File>
<File
RelativePath=".\src\paths.h"
>
diff --git a/src/hungarian.c b/src/hungarian.c
new file mode 100644
index 0000000..824af3c
--- /dev/null
+++ b/src/hungarian.c
@@ -0,0 +1,419 @@
+/********************************************************************
+ ********************************************************************
+ **
+ ** libhungarian by Cyrill Stachniss, 2004
+ **
+ **
+ ** Solving the Minimum Assignment Problem using the
+ ** Hungarian Method.
+ **
+ ** ** This file may be freely copied and distributed! **
+ **
+ ** Parts of the used code was originally provided by the
+ ** "Stanford GraphGase", but I made changes to this code.
+ ** As asked by the copyright node of the "Stanford GraphGase",
+ ** I hereby proclaim that this file are *NOT* part of the
+ ** "Stanford GraphGase" distrubition!
+ **
+ ** This file is distributed in the hope that it will be useful,
+ ** but WITHOUT ANY WARRANTY; without even the implied
+ ** warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ ** PURPOSE.
+ **
+ ********************************************************************
+ ********************************************************************/
+
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "hungarian.h"
+
+#define INF (0x7FFFFFFF)
+#define verbose (0)
+
+#define hungarian_test_alloc(X) do {if ((void *)(X) == NULL) fprintf(stderr, "Out of memory in %s, (%s, line %d).\n", __FUNCTION__, __FILE__, __LINE__); } while (0)
+
+
+void hungarian_print_matrix(int** C, int rows, int cols) {
+ int i,j;
+ fprintf(stderr , "\n");
+ for(i=0; i<rows; i++) {
+ fprintf(stderr, " [");
+ for(j=0; j<cols; j++) {
+ fprintf(stderr, "%5d ",C[i][j]);
+ }
+ fprintf(stderr, "]\n");
+ }
+ fprintf(stderr, "\n");
+}
+
+void hungarian_print_assignment(hungarian_problem_t* p) {
+ hungarian_print_matrix(p->assignment, p->num_rows, p->num_cols) ;
+}
+
+void hungarian_print_costmatrix(hungarian_problem_t* p) {
+ hungarian_print_matrix(p->cost, p->num_rows, p->num_cols) ;
+}
+
+void hungarian_print_status(hungarian_problem_t* p) {
+
+ fprintf(stderr,"cost:\n");
+ hungarian_print_costmatrix(p);
+
+ fprintf(stderr,"assignment:\n");
+ hungarian_print_assignment(p);
+
+}
+
+int hungarian_imax(int a, int b) {
+ return (a<b)?b:a;
+}
+
+int hungarian_init(hungarian_problem_t* p, int** cost_matrix, int rows, int cols, int mode) {
+
+ int i,j, org_cols, org_rows;
+ int max_cost;
+ max_cost = 0;
+
+ org_cols = cols;
+ org_rows = rows;
+
+ // is the number of cols not equal to number of rows ?
+ // if yes, expand with 0-cols / 0-cols
+ rows = hungarian_imax(cols, rows);
+ cols = rows;
+
+ p->num_rows = rows;
+ p->num_cols = cols;
+
+ p->cost = (int**)calloc(rows,sizeof(int*));
+ hungarian_test_alloc(p->cost);
+ p->assignment = (int**)calloc(rows,sizeof(int*));
+ hungarian_test_alloc(p->assignment);
+
+ for(i=0; i<p->num_rows; i++) {
+ p->cost[i] = (int*)calloc(cols,sizeof(int));
+ hungarian_test_alloc(p->cost[i]);
+ p->assignment[i] = (int*)calloc(cols,sizeof(int));
+ hungarian_test_alloc(p->assignment[i]);
+ for(j=0; j<p->num_cols; j++) {
+ p->cost[i][j] = (i < org_rows && j < org_cols) ? cost_matrix[i][j] : 0;
+ p->assignment[i][j] = 0;
+
+ if (max_cost < p->cost[i][j])
+ max_cost = p->cost[i][j];
+ }
+ }
+
+
+ if (mode == HUNGARIAN_MODE_MAXIMIZE_UTIL) {
+ for(i=0; i<p->num_rows; i++) {
+ for(j=0; j<p->num_cols; j++) {
+ p->cost[i][j] = max_cost - p->cost[i][j];
+ }
+ }
+ }
+ else if (mode == HUNGARIAN_MODE_MINIMIZE_COST) {
+ // nothing to do
+ }
+ else
+ fprintf(stderr,"%s: unknown mode. Mode was set to HUNGARIAN_MODE_MINIMIZE_COST !\n", __FUNCTION__);
+
+ return rows;
+}
+
+
+
+
+void hungarian_free(hungarian_problem_t* p) {
+ int i;
+ for(i=0; i<p->num_rows; i++) {
+ free(p->cost[i]);
+ free(p->assignment[i]);
+ }
+ free(p->cost);
+ free(p->assignment);
+ p->cost = NULL;
+ p->assignment = NULL;
+}
+
+
+
+void hungarian_solve(hungarian_problem_t* p)
+{
+ int i, j, m, n, k, l, s, t, q, unmatched, cost;
+ int* col_mate;
+ int* row_mate;
+ int* parent_row;
+ int* unchosen_row;
+ int* row_dec;
+ int* col_inc;
+ int* slack;
+ int* slack_row;
+
+ cost=0;
+ m =p->num_rows;
+ n =p->num_cols;
+
+ col_mate = (int*)calloc(p->num_rows,sizeof(int));
+ hungarian_test_alloc(col_mate);
+ unchosen_row = (int*)calloc(p->num_rows,sizeof(int));
+ hungarian_test_alloc(unchosen_row);
+ row_dec = (int*)calloc(p->num_rows,sizeof(int));
+ hungarian_test_alloc(row_dec);
+ slack_row = (int*)calloc(p->num_rows,sizeof(int));
+ hungarian_test_alloc(slack_row);
+
+ row_mate = (int*)calloc(p->num_cols,sizeof(int));
+ hungarian_test_alloc(row_mate);
+ parent_row = (int*)calloc(p->num_cols,sizeof(int));
+ hungarian_test_alloc(parent_row);
+ col_inc = (int*)calloc(p->num_cols,sizeof(int));
+ hungarian_test_alloc(col_inc);
+ slack = (int*)calloc(p->num_cols,sizeof(int));
+ hungarian_test_alloc(slack);
+
+ for (i=0;i<p->num_rows;i++) {
+ col_mate[i]=0;
+ unchosen_row[i]=0;
+ row_dec[i]=0;
+ slack_row[i]=0;
+ }
+ for (j=0;j<p->num_cols;j++) {
+ row_mate[j]=0;
+ parent_row[j] = 0;
+ col_inc[j]=0;
+ slack[j]=0;
+ }
+
+ for (i=0;i<p->num_rows;++i)
+ for (j=0;j<p->num_cols;++j)
+ p->assignment[i][j]=HUNGARIAN_NOT_ASSIGNED;
+
+ // Begin subtract column minima in order to start with lots of zeroes 12
+ if (verbose)
+ fprintf(stderr, "Using heuristic\n");
+ for (l=0;l<n;l++)
+ {
+ s=p->cost[0][l];
+ for (k=1;k<m;k++)
+ if (p->cost[k][l]<s)
+ s=p->cost[k][l];
+ cost+=s;
+ if (s!=0)
+ for (k=0;k<m;k++)
+ p->cost[k][l]-=s;
+ }
+ // End subtract column minima in order to start with lots of zeroes 12
+
+ // Begin initial state 16
+ t=0;
+ for (l=0;l<n;l++)
+ {
+ row_mate[l]= -1;
+ parent_row[l]= -1;
+ col_inc[l]=0;
+ slack[l]=INF;
+ }
+ for (k=0;k<m;k++)
+ {
+ s=p->cost[k][0];
+ for (l=1;l<n;l++)
+ if (p->cost[k][l]<s)
+ s=p->cost[k][l];
+ row_dec[k]=s;
+ for (l=0;l<n;l++)
+ if (s==p->cost[k][l] && row_mate[l]<0)
+ {
+ col_mate[k]=l;
+ row_mate[l]=k;
+ if (verbose)
+ fprintf(stderr, "matching col %d==row %d\n",l,k);
+ goto row_done;
+ }
+ col_mate[k]= -1;
+ if (verbose)
+ fprintf(stderr, "node %d: unmatched row %d\n",t,k);
+ unchosen_row[t++]=k;
+ row_done:
+ ;
+ }
+ // End initial state 16
+
+ // Begin Hungarian algorithm 18
+ if (t==0)
+ goto done;
+ unmatched=t;
+ while (1)
+ {
+ if (verbose)
+ fprintf(stderr, "Matched %d rows.\n",m-t);
+ q=0;
+ while (1)
+ {
+ while (q<t)
+ {
+ // Begin explore node q of the forest 19
+ {
+ k=unchosen_row[q];
+ s=row_dec[k];
+ for (l=0;l<n;l++)
+ if (slack[l])
+ {
+ int del;
+ del=p->cost[k][l]-s+col_inc[l];
+ if (del<slack[l])
+ {
+ if (del==0)
+ {
+ if (row_mate[l]<0)
+ goto breakthru;
+ slack[l]=0;
+ parent_row[l]=k;
+ if (verbose)
+ fprintf(stderr, "node %d: row %d==col %d--row %d\n",
+ t,row_mate[l],l,k);
+ unchosen_row[t++]=row_mate[l];
+ }
+ else
+ {
+ slack[l]=del;
+ slack_row[l]=k;
+ }
+ }
+ }
+ }
+ // End explore node q of the forest 19
+ q++;
+ }
+
+ // Begin introduce a new zero into the matrix 21
+ s=INF;
+ for (l=0;l<n;l++)
+ if (slack[l] && slack[l]<s)
+ s=slack[l];
+ for (q=0;q<t;q++)
+ row_dec[unchosen_row[q]]+=s;
+ for (l=0;l<n;l++)
+ if (slack[l])
+ {
+ slack[l]-=s;
+ if (slack[l]==0)
+ {
+ // Begin look at a new zero 22
+ k=slack_row[l];
+ if (verbose)
+ fprintf(stderr,
+ "Decreasing uncovered elements by %d produces zero at [%d,%d]\n",
+ s,k,l);
+ if (row_mate[l]<0)
+ {
+ for (j=l+1;j<n;j++)
+ if (slack[j]==0)
+ col_inc[j]+=s;
+ goto breakthru;
+ }
+ else
+ {
+ parent_row[l]=k;
+ if (verbose)
+ fprintf(stderr, "node %d: row %d==col %d--row %d\n",t,row_mate[l],l,k);
+ unchosen_row[t++]=row_mate[l];
+ }
+ // End look at a new zero 22
+ }
+ }
+ else
+ col_inc[l]+=s;
+ // End introduce a new zero into the matrix 21
+ }
+ breakthru:
+ // Begin update the matching 20
+ if (verbose)
+ fprintf(stderr, "Breakthrough at node %d of %d!\n",q,t);
+ while (1)
+ {
+ j=col_mate[k];
+ col_mate[k]=l;
+ row_mate[l]=k;
+ if (verbose)
+ fprintf(stderr, "rematching col %d==row %d\n",l,k);
+ if (j<0)
+ break;
+ k=parent_row[j];
+ l=j;
+ }
+ // End update the matching 20
+ if (--unmatched==0)
+ goto done;
+ // Begin get ready for another stage 17
+ t=0;
+ for (l=0;l<n;l++)
+ {
+ parent_row[l]= -1;
+ slack[l]=INF;
+ }
+ for (k=0;k<m;k++)
+ if (col_mate[k]<0)
+ {
+ if (verbose)
+ fprintf(stderr, "node %d: unmatched row %d\n",t,k);
+ unchosen_row[t++]=k;
+ }
+ // End get ready for another stage 17
+ }
+ done:
+
+ // Begin doublecheck the solution 23
+ for (k=0;k<m;k++)
+ for (l=0;l<n;l++)
+ if (p->cost[k][l]<row_dec[k]-col_inc[l])
+ exit(0);
+ for (k=0;k<m;k++)
+ {
+ l=col_mate[k];
+ if (l<0 || p->cost[k][l]!=row_dec[k]-col_inc[l])
+ exit(0);
+ }
+ k=0;
+ for (l=0;l<n;l++)
+ if (col_inc[l])
+ k++;
+ if (k>m)
+ exit(0);
+ // End doublecheck the solution 23
+ // End Hungarian algorithm 18
+
+ for (i=0;i<m;++i)
+ {
+ p->assignment[i][col_mate[i]]=HUNGARIAN_ASSIGNED;
+ /*TRACE("%d - %d\n", i, col_mate[i]);*/
+ }
+ for (k=0;k<m;++k)
+ {
+ for (l=0;l<n;++l)
+ {
+ /*TRACE("%d ",p->cost[k][l]-row_dec[k]+col_inc[l]);*/
+ p->cost[k][l]=p->cost[k][l]-row_dec[k]+col_inc[l];
+ }
+ /*TRACE("\n");*/
+ }
+ for (i=0;i<m;i++)
+ cost+=row_dec[i];
+ for (i=0;i<n;i++)
+ cost-=col_inc[i];
+ if (verbose)
+ fprintf(stderr, "Cost is %d\n",cost);
+
+
+ free(slack);
+ free(col_inc);
+ free(parent_row);
+ free(row_mate);
+ free(slack_row);
+ free(row_dec);
+ free(unchosen_row);
+ free(col_mate);
+}
+
+
diff --git a/src/hungarian.h b/src/hungarian.h
new file mode 100644
index 0000000..0cc207b
--- /dev/null
+++ b/src/hungarian.h
@@ -0,0 +1,78 @@
+/********************************************************************
+ ********************************************************************
+ **
+ ** libhungarian by Cyrill Stachniss, 2004
+ **
+ **
+ ** Solving the Minimum Assignment Problem using the
+ ** Hungarian Method.
+ **
+ ** ** This file may be freely copied and distributed! **
+ **
+ ** Parts of the used code was originally provided by the
+ ** "Stanford GraphGase", but I made changes to this code.
+ ** As asked by the copyright node of the "Stanford GraphGase",
+ ** I hereby proclaim that this file are *NOT* part of the
+ ** "Stanford GraphGase" distrubition!
+ **
+ ** This file is distributed in the hope that it will be useful,
+ ** but WITHOUT ANY WARRANTY; without even the implied
+ ** warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ ** PURPOSE.
+ **
+ ********************************************************************
+ ********************************************************************/
+
+#ifndef HUNGARIAN_H
+#define HUNGARIAN_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define HUNGARIAN_NOT_ASSIGNED 0
+#define HUNGARIAN_ASSIGNED 1
+
+#define HUNGARIAN_MODE_MINIMIZE_COST 0
+#define HUNGARIAN_MODE_MAXIMIZE_UTIL 1
+
+
+typedef struct {
+ int num_rows;
+ int num_cols;
+ int** cost;
+ int** assignment;
+} hungarian_problem_t;
+
+/** This method initialize the hungarian_problem structure and init
+ * the cost matrices (missing lines or columns are filled with 0).
+ * It returns the size of the quadratic(!) assignment matrix. **/
+int hungarian_init(hungarian_problem_t* p,
+ int** cost_matrix,
+ int rows,
+ int cols,
+ int mode);
+
+/** Free the memory allocated by init. **/
+void hungarian_free(hungarian_problem_t* p);
+
+/** This method computes the optimal assignment. **/
+void hungarian_solve(hungarian_problem_t* p);
+
+/** Print the computed optimal assignment. **/
+void hungarian_print_assignment(hungarian_problem_t* p);
+
+/** Print the cost matrix. **/
+void hungarian_print_costmatrix(hungarian_problem_t* p);
+
+/** Print cost matrix and assignment matrix. **/
+void hungarian_print_status(hungarian_problem_t* p);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
+
+
diff --git a/src/munkres/matrix.cpp b/src/munkres/matrix.cpp
new file mode 100644
index 0000000..22c3f50
--- /dev/null
+++ b/src/munkres/matrix.cpp
@@ -0,0 +1,231 @@
+/*
+ * Copyright (c) 2007 John Weaver
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include "matrix.h"
+
+#include <cassert>
+#include <cstdlib>
+#include <algorithm>
+
+/*export*/ template <class T>
+Matrix<T>::Matrix() {
+ m_rows = 0;
+ m_columns = 0;
+ m_matrix = NULL;
+}
+
+/*export*/ template <class T>
+Matrix<T>::Matrix(const Matrix<T> &other) {
+ if ( other.m_matrix != NULL ) {
+ // copy arrays
+ m_matrix = NULL;
+ resize(other.m_rows, other.m_columns);
+ for ( int i = 0 ; i < m_rows ; i++ )
+ for ( int j = 0 ; j < m_columns ; j++ )
+ m_matrix[i][j] = other.m_matrix[i][j];
+ } else {
+ m_matrix = NULL;
+ m_rows = 0;
+ m_columns = 0;
+ }
+}
+
+/*export*/ template <class T>
+Matrix<T>::Matrix(int rows, int columns) {
+ m_matrix = NULL;
+ resize(rows, columns);
+}
+
+/*export*/ template <class T>
+Matrix<T> &
+Matrix<T>::operator= (const Matrix<T> &other) {
+ if ( other.m_matrix != NULL ) {
+ // copy arrays
+ resize(other.m_rows, other.m_columns);
+ for ( int i = 0 ; i < m_rows ; i++ )
+ for ( int j = 0 ; j < m_columns ; j++ )
+ m_matrix[i][j] = other.m_matrix[i][j];
+ } else {
+ // free arrays
+ for ( int i = 0 ; i < m_columns ; i++ )
+ delete [] m_matrix[i];
+
+ delete [] m_matrix;
+
+ m_matrix = NULL;
+ m_rows = 0;
+ m_columns = 0;
+ }
+
+ return *this;
+}
+
+/*export*/ template <class T>
+Matrix<T>::~Matrix() {
+ if ( m_matrix != NULL ) {
+ // free arrays
+ for ( int i = 0 ; i < m_rows ; i++ )
+ delete [] m_matrix[i];
+
+ delete [] m_matrix;
+ }
+ m_matrix = NULL;
+}
+
+/*export*/ template <class T>
+void
+Matrix<T>::resize(int rows, int columns) {
+ if ( m_matrix == NULL ) {
+ // alloc arrays
+ m_matrix = new T*[rows]; // rows
+ for ( int i = 0 ; i < rows ; i++ )
+ m_matrix[i] = new T[columns]; // columns
+
+ m_rows = rows;
+ m_columns = columns;
+ clear();
+ } else {
+ // save array pointer
+ T **new_matrix;
+ // alloc new arrays
+ new_matrix = new T*[rows]; // rows
+ for ( int i = 0 ; i < rows ; i++ ) {
+ new_matrix[i] = new T[columns]; // columns
+ for ( int j = 0 ; j < columns ; j++ )
+ new_matrix[i][j] = 0;
+ }
+
+ // copy data from saved pointer to new arrays
+ int minrows = std::min<int>(rows, m_rows);
+ int mincols = std::min<int>(columns, m_columns);
+ for ( int x = 0 ; x < minrows ; x++ )
+ for ( int y = 0 ; y < mincols ; y++ )
+ new_matrix[x][y] = m_matrix[x][y];
+
+ // delete old arrays
+ if ( m_matrix != NULL ) {
+ for ( int i = 0 ; i < m_rows ; i++ )
+ delete [] m_matrix[i];
+
+ delete [] m_matrix;
+ }
+
+ m_matrix = new_matrix;
+ }
+
+ m_rows = rows;
+ m_columns = columns;
+}
+
+/*export*/ template <class T>
+void
+Matrix<T>::identity() {
+ assert( m_matrix != NULL );
+
+ clear();
+
+ int x = std::min<int>(m_rows, m_columns);
+ for ( int i = 0 ; i < x ; i++ )
+ m_matrix[i][i] = 1;
+}
+
+/*export*/ template <class T>
+void
+Matrix<T>::clear() {
+ assert( m_matrix != NULL );
+
+ for ( int i = 0 ; i < m_rows ; i++ )
+ for ( int j = 0 ; j < m_columns ; j++ )
+ m_matrix[i][j] = 0;
+}
+
+/*export*/ template <class T>
+T
+Matrix<T>::trace() {
+ assert( m_matrix != NULL );
+
+ T value = 0;
+
+ int x = std::min<int>(m_rows, m_columns);
+ for ( int i = 0 ; i < x ; i++ )
+ value += m_matrix[i][i];
+
+ return value;
+}
+
+/*export*/ template <class T>
+Matrix<T>&
+Matrix<T>::transpose() {
+ assert( m_rows > 0 );
+ assert( m_columns > 0 );
+
+ int new_rows = m_columns;
+ int new_columns = m_rows;
+
+ if ( m_rows != m_columns ) {
+ // expand matrix
+ int m = std::max<int>(m_rows, m_columns);
+ resize(m,m);
+ }
+
+ for ( int i = 0 ; i < m_rows ; i++ ) {
+ for ( int j = i+1 ; j < m_columns ; j++ ) {
+ T tmp = m_matrix[i][j];
+ m_matrix[i][j] = m_matrix[j][i];
+ m_matrix[j][i] = tmp;
+ }
+ }
+
+ if ( new_columns != new_rows ) {
+ // trim off excess.
+ resize(new_rows, new_columns);
+ }
+
+ return *this;
+}
+
+/*export*/ template <class T>
+Matrix<T>
+Matrix<T>::product(Matrix<T> &other) {
+ assert( m_matrix != NULL );
+ assert( other.m_matrix != NULL );
+ assert ( m_columns == other.m_rows );
+
+ Matrix<T> out(m_rows, other.m_columns);
+
+ for ( int i = 0 ; i < out.m_rows ; i++ ) {
+ for ( int j = 0 ; j < out.m_columns ; j++ ) {
+ for ( int x = 0 ; x < m_columns ; x++ ) {
+ out(i,j) += m_matrix[i][x] * other.m_matrix[x][j];
+ }
+ }
+ }
+
+ return out;
+}
+
+/*export*/ template <class T>
+T&
+Matrix<T>::operator ()(int x, int y) {
+ assert ( x >= 0 );
+ assert ( y >= 0 );
+ assert ( x < m_rows );
+ assert ( y < m_columns );
+ assert ( m_matrix != NULL );
+ return m_matrix[x][y];
+}
diff --git a/src/munkres/matrix.h b/src/munkres/matrix.h
new file mode 100644
index 0000000..6975e3a
--- /dev/null
+++ b/src/munkres/matrix.h
@@ -0,0 +1,59 @@
+/*
+ * Copyright (c) 2007 John Weaver
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#if !defined(_MATRIX_H_)
+#define _MATRIX_H_
+
+template <class T>
+class Matrix {
+public:
+ Matrix();
+ Matrix(int rows, int columns);
+ Matrix(const Matrix<T> &other);
+ Matrix<T> & operator= (const Matrix<T> &other);
+ ~Matrix();
+ // all operations except product modify the matrix in-place.
+ void resize(int rows, int columns);
+ void identity(void);
+ void clear(void);
+ T& operator () (int x, int y);
+ T trace(void);
+ Matrix<T>& transpose(void);
+ Matrix<T> product(Matrix<T> &other);
+ int minsize(void) {
+ return ((m_rows < m_columns) ? m_rows : m_columns);
+ }
+ int columns(void) {
+ return m_columns;
+ }
+ int rows(void) {
+ return m_rows;
+ }
+private:
+ T **m_matrix;
+ int m_rows;
+ int m_columns;
+};
+
+#ifndef USE_EXPORT_KEYWORD
+#include "matrix.cpp"
+//#define export /*export*/
+#endif
+
+#endif /* !defined(_MATRIX_H_) */
+
diff --git a/src/munkres/munkres.cpp b/src/munkres/munkres.cpp
new file mode 100644
index 0000000..8b0aba7
--- /dev/null
+++ b/src/munkres/munkres.cpp
@@ -0,0 +1,336 @@
+/*
+ * Copyright (c) 2007 John Weaver
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include "munkres.h"
+
+#include <iostream>
+
+#define Z_NORMAL 0
+#define Z_STAR 1
+#define Z_PRIME 2
+
+bool
+Munkres::find_uncovered_in_matrix(double item, int &row, int &col) {
+ for ( row = 0 ; row < matrix.rows() ; row++ )
+ if ( !row_mask[row] )
+ for ( col = 0 ; col < matrix.columns() ; col++ )
+ if ( !col_mask[col] )
+ if ( matrix(row,col) == item )
+ return true;
+
+ return false;
+}
+
+bool
+Munkres::pair_in_list(const std::pair<int,int> &needle, const std::list<std::pair<int,int> > &haystack) {
+ for ( std::list<std::pair<int,int> >::const_iterator i = haystack.begin() ; i != haystack.end() ; i++ ) {
+ if ( needle == *i )
+ return true;
+ }
+
+ return false;
+}
+
+int
+Munkres::step1(void) {
+ for ( int row = 0 ; row < matrix.rows() ; row++ )
+ for ( int col = 0 ; col < matrix.columns() ; col++ )
+ if ( matrix(row,col) == 0 ) {
+ bool isstarred = false;
+ for ( int nrow = 0 ; nrow < matrix.rows() ; nrow++ )
+ if ( mask_matrix(nrow,col) == Z_STAR )
+ isstarred = true;
+
+ if ( !isstarred ) {
+ for ( int ncol = 0 ; ncol < matrix.columns() ; ncol++ )
+ if ( mask_matrix(row,ncol) == Z_STAR )
+ isstarred = true;
+ }
+
+ if ( !isstarred ) {
+ mask_matrix(row,col) = Z_STAR;
+ }
+ }
+
+ return 2;
+}
+
+int
+Munkres::step2(void) {
+ int covercount = 0;
+ for ( int row = 0 ; row < matrix.rows() ; row++ )
+ for ( int col = 0 ; col < matrix.columns() ; col++ )
+ if ( mask_matrix(row,col) == Z_STAR ) {
+ col_mask[col] = true;
+ covercount++;
+ }
+
+ int k = matrix.minsize();
+
+ if ( covercount >= k ) {
+#ifdef DEBUG
+ std::cout << "Final cover count: " << covercount << std::endl;
+#endif
+ return 0;
+ }
+
+#ifdef DEBUG
+ std::cout << "Munkres matrix has " << covercount << " of " << k << " Columns covered:" << std::endl;
+ for ( int row = 0 ; row < matrix.rows() ; row++ ) {
+ for ( int col = 0 ; col < matrix.columns() ; col++ ) {
+ std::cout.width(8);
+ std::cout << matrix(row,col) << ",";
+ }
+ std::cout << std::endl;
+ }
+ std::cout << std::endl;
+#endif
+
+
+ return 3;
+}
+
+int
+Munkres::step3(void) {
+ /*
+ Main Zero Search
+
+ 1. Find an uncovered Z in the distance matrix and prime it. If no such zero exists, go to Step 5
+ 2. If No Z* exists in the row of the Z', go to Step 4.
+ 3. If a Z* exists, cover this row and uncover the column of the Z*. Return to Step 3.1 to find a new Z
+ */
+ if ( find_uncovered_in_matrix(0, saverow, savecol) ) {
+ mask_matrix(saverow,savecol) = Z_PRIME; // prime it.
+ } else {
+ return 5;
+ }
+
+ for ( int ncol = 0 ; ncol < matrix.columns() ; ncol++ )
+ if ( mask_matrix(saverow,ncol) == Z_STAR ) {
+ row_mask[saverow] = true; //cover this row and
+ col_mask[ncol] = false; // uncover the column containing the starred zero
+ return 3; // repeat
+ }
+
+ return 4; // no starred zero in the row containing this primed zero
+}
+
+int
+Munkres::step4(void) {
+ std::list<std::pair<int,int> > seq;
+ // use saverow, savecol from step 3.
+ std::pair<int,int> z0(saverow, savecol);
+ std::pair<int,int> z1(-1,-1);
+ std::pair<int,int> z2n(-1,-1);
+ seq.insert(seq.end(), z0);
+ int row, col = savecol;
+ /*
+ Increment Set of Starred Zeros
+
+ 1. Construct the ``alternating sequence'' of primed and starred zeros:
+
+ Z0 : Unpaired Z' from Step 4.2
+ Z1 : The Z* in the column of Z0
+ Z[2N] : The Z' in the row of Z[2N-1], if such a zero exists
+ Z[2N+1] : The Z* in the column of Z[2N]
+
+ The sequence eventually terminates with an unpaired Z' = Z[2N] for some N.
+ */
+ bool madepair;
+ do {
+ madepair = false;
+ for ( row = 0 ; row < matrix.rows() ; row++ )
+ if ( mask_matrix(row,col) == Z_STAR ) {
+ z1.first = row;
+ z1.second = col;
+ if ( pair_in_list(z1, seq) )
+ continue;
+
+ madepair = true;
+ seq.insert(seq.end(), z1);
+ break;
+ }
+
+ if ( !madepair )
+ break;
+
+ madepair = false;
+
+ for ( col = 0 ; col < matrix.columns() ; col++ )
+ if ( mask_matrix(row,col) == Z_PRIME ) {
+ z2n.first = row;
+ z2n.second = col;
+ if ( pair_in_list(z2n, seq) )
+ continue;
+ madepair = true;
+ seq.insert(seq.end(), z2n);
+ break;
+ }
+ } while ( madepair );
+
+ for ( std::list<std::pair<int,int> >::iterator i = seq.begin() ;
+ i != seq.end() ;
+ i++ ) {
+ // 2. Unstar each starred zero of the sequence.
+ if ( mask_matrix(i->first,i->second) == Z_STAR )
+ mask_matrix(i->first,i->second) = Z_NORMAL;
+
+ // 3. Star each primed zero of the sequence,
+ // thus increasing the number of starred zeros by one.
+ if ( mask_matrix(i->first,i->second) == Z_PRIME )
+ mask_matrix(i->first,i->second) = Z_STAR;
+ }
+
+ // 4. Erase all primes, uncover all columns and rows,
+ for ( int row = 0 ; row < mask_matrix.rows() ; row++ )
+ for ( int col = 0 ; col < mask_matrix.columns() ; col++ )
+ if ( mask_matrix(row,col) == Z_PRIME )
+ mask_matrix(row,col) = Z_NORMAL;
+
+ for ( int i = 0 ; i < matrix.rows() ; i++ ) {
+ row_mask[i] = false;
+ }
+
+ for ( int i = 0 ; i < matrix.columns() ; i++ ) {
+ col_mask[i] = false;
+ }
+
+ // and return to Step 2.
+ return 2;
+}
+
+int
+Munkres::step5(void) {
+ /*
+ New Zero Manufactures
+
+ 1. Let h be the smallest uncovered entry in the (modified) distance matrix.
+ 2. Add h to all covered rows.
+ 3. Subtract h from all uncovered columns
+ 4. Return to Step 3, without altering stars, primes, or covers.
+ */
+ double h = 0;
+ for ( int row = 0 ; row < matrix.rows() ; row++ ) {
+ if ( !row_mask[row] ) {
+ for ( int col = 0 ; col < matrix.columns() ; col++ ) {
+ if ( !col_mask[col] ) {
+ if ( (h > matrix(row,col) && matrix(row,col) != 0) || h == 0 ) {
+ h = matrix(row,col);
+ }
+ }
+ }
+ }
+ }
+
+ for ( int row = 0 ; row < matrix.rows() ; row++ )
+ for ( int col = 0 ; col < matrix.columns() ; col++ ) {
+ if ( row_mask[row] )
+ matrix(row,col) += h;
+
+ if ( !col_mask[col] )
+ matrix(row,col) -= h;
+ }
+
+ return 3;
+}
+
+void
+Munkres::solve(Matrix<double> &m) {
+ // Linear assignment problem solution
+ // [modifies matrix in-place.]
+ // matrix(row,col): row major format assumed.
+
+ // Assignments are remaining 0 values
+ // (extra 0 values are replaced with -1)
+#ifdef DEBUG
+ std::cout << "Munkres input matrix:" << std::endl;
+ for ( int row = 0 ; row < m.rows() ; row++ ) {
+ for ( int col = 0 ; col < m.columns() ; col++ ) {
+ std::cout.width(8);
+ std::cout << m(row,col) << ",";
+ }
+ std::cout << std::endl;
+ }
+ std::cout << std::endl;
+#endif
+
+ bool notdone = true;
+ int step = 1;
+
+ this->matrix = m;
+ // Z_STAR == 1 == starred, Z_PRIME == 2 == primed
+ mask_matrix.resize(matrix.rows(), matrix.columns());
+
+ row_mask = new bool[matrix.rows()];
+ col_mask = new bool[matrix.columns()];
+ for ( int i = 0 ; i < matrix.rows() ; i++ ) {
+ row_mask[i] = false;
+ }
+
+ for ( int i = 0 ; i < matrix.columns() ; i++ ) {
+ col_mask[i] = false;
+ }
+
+ while ( notdone ) {
+ switch ( step ) {
+ case 0:
+ notdone = false;
+ break;
+ case 1:
+ step = step1();
+ break;
+ case 2:
+ step = step2();
+ break;
+ case 3:
+ step = step3();
+ break;
+ case 4:
+ step = step4();
+ break;
+ case 5:
+ step = step5();
+ break;
+ }
+ }
+
+ // Store results
+ for ( int row = 0 ; row < matrix.rows() ; row++ )
+ for ( int col = 0 ; col < matrix.columns() ; col++ )
+ if ( mask_matrix(row,col) == Z_STAR )
+ matrix(row,col) = 0;
+ else
+ matrix(row,col) = -1;
+
+#ifdef DEBUG
+ std::cout << "Munkres output matrix:" << std::endl;
+ for ( int row = 0 ; row < matrix.rows() ; row++ ) {
+ for ( int col = 0 ; col < matrix.columns() ; col++ ) {
+ std::cout.width(1);
+ std::cout << matrix(row,col) << ",";
+ }
+ std::cout << std::endl;
+ }
+ std::cout << std::endl;
+#endif
+
+ m = matrix;
+
+ delete [] row_mask;
+ delete [] col_mask;
+}
diff --git a/src/munkres/munkres.h b/src/munkres/munkres.h
new file mode 100644
index 0000000..4f233ac
--- /dev/null
+++ b/src/munkres/munkres.h
@@ -0,0 +1,46 @@
+/*
+ * Copyright (c) 2007 John Weaver
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#if !defined(_MUNKRES_H_)
+#define _MUNKRES_H_
+
+#include "matrix.h"
+
+#include <list>
+#include <utility>
+
+class Munkres {
+public:
+ void solve(Matrix<double> &m);
+private:
+ inline bool find_uncovered_in_matrix(double,int&,int&);
+ inline bool pair_in_list(const std::pair<int,int> &, const std::list<std::pair<int,int> > &);
+ int step1(void);
+ int step2(void);
+ int step3(void);
+ int step4(void);
+ int step5(void);
+ int step6(void);
+ Matrix<int> mask_matrix;
+ Matrix<double> matrix;
+ bool *row_mask;
+ bool *col_mask;
+ int saverow, savecol;
+};
+
+#endif /* !defined(_MUNKRES_H_) */
diff --git a/src/tracker.cpp b/src/tracker.cpp
index 8e9abee..9eaa495 100644
--- a/src/tracker.cpp
+++ b/src/tracker.cpp
@@ -1,5 +1,8 @@
#include "tracker.h"
+#include "matrix.h"
+#include "munkres.h"
+
//Global variables
int gNumFrames;
int gMaxDetections;
@@ -218,6 +221,38 @@ void patchUpResults()
printf("Finished: Patched %d matches\n", totalPatches);
}
+void dbgPrintCostMatrix(char* fname, Matrix<double>& costMatrix, std::vector<int>& costSrcGIdxList, std::vector<int>& costDestGIdxList)
+{
+ FILE* debugfp = fopen(fname, "w");
+
+ fprintf(debugfp, " %5c", ' ');
+ for ( int j=0; j < costMatrix.columns(); ++j )
+ {
+ if ( j < costSrcGIdxList.size() )
+ fprintf(debugfp ,"%9d ", costDestGIdxList[j]);
+ else
+ fprintf(debugfp ,"%9s ", "##");
+ }
+ fprintf(debugfp, "\n");
+
+ for ( int i=0; i < costMatrix.rows(); ++i )
+ {
+
+ if ( i < costSrcGIdxList.size() )
+ fprintf(debugfp, "%5d: ", costSrcGIdxList[i]);
+ else
+ fprintf(debugfp, "%5s: ", "##");
+
+ for ( int j=0; j < costMatrix.columns(); ++j )
+ {
+ fprintf(debugfp ,"%9.5g ", costMatrix(i, j));
+ }
+ fprintf(debugfp, "\n");
+ }
+
+ fclose(debugfp);
+}
+
int main(int argc, char* argv[])
{
int outputargidx = ReadDetectionData(argc, argv);
@@ -228,6 +263,13 @@ int main(int argc, char* argv[])
CSourcePath** outEdges = new CSourcePath*[gMaxDetections];
std::vector<CSourcePath*>* inEdges = new std::vector<CSourcePath*>[gMaxDetections];
+ std::vector<int> costSrcGIdxList;
+ std::vector<int> costDestGIdxList;
+ std::set<int> srcInclSet;
+
+ costSrcGIdxList.reserve(gMaxDetections);
+ costDestGIdxList.reserve(gMaxDetections);
+
for ( int t=0; t < gNumFrames-1; ++t )
{
printf("t = %d, %d detections\n", t, rgDetectLengths[t]);
@@ -235,43 +277,94 @@ int main(int argc, char* argv[])
ClearEdges(inEdges, outEdges);
BuildBestPaths(inEdges, outEdges, t);
+ // Note: BuildBestPaths returns only the BEST edge in inEdges, not all edges coming into a node
+ // will ONLY get multiple entries when occlusion code returns incomin edges as well.
+
//Occlusions
for ( int iLookback=1; iLookback <= 5; ++iLookback )
{
BuildBestPaths(inEdges, outEdges, t, iLookback);
}
+ costSrcGIdxList.clear();
+ costDestGIdxList.clear();
+ srcInclSet.clear();
for ( int destPtIdx=0; destPtIdx < rgDetectLengths[t+1]; ++destPtIdx)
{
- int dbgDestGIdx = GetGlobalIdx(t+1, destPtIdx);
-
if ( inEdges[destPtIdx].size() == 0 )
continue;
- int bestTrackletIdx = FindMinCostIdx(inEdges[destPtIdx]);
- if ( bestTrackletIdx < 0 )
- continue;
-
- int newTrackletID = inEdges[destPtIdx][bestTrackletIdx]->trackletID;
+ int destGIdx = GetGlobalIdx(t+1, destPtIdx);
+ costDestGIdxList.push_back(destGIdx);
- if ( newTrackletID < 0 )
+ std::map<int,CSourcePath*>::iterator srcPtsIter = gConnectIn[destGIdx].begin();
+ while( srcPtsIter != gConnectIn[destGIdx].end() )
{
- //Add new tracklet to list etc. and set id
- newTrackletID = gAssignedTracklets.size();
- inEdges[destPtIdx][bestTrackletIdx]->trackletID = newTrackletID;
+ int srcGIdx = srcPtsIter->first;
+ if ( srcInclSet.count(srcGIdx) == 0 )
+ {
+ costSrcGIdxList.push_back(srcGIdx);
+ srcInclSet.insert(srcGIdx);
+ }
+ ++srcPtsIter;
+ }
+ }
- tPathList newList;
- gAssignedTracklets.push_back(newList);
+ Matrix<double> costMatrix(costSrcGIdxList.size(), costDestGIdxList.size());
+
+ for ( int i=0; i < costSrcGIdxList.size(); ++i )
+ {
+ int srcGIdx = costSrcGIdxList[i];
+ for ( int j=0; j < costDestGIdxList.size(); ++j )
+ {
+ int destGIdx = costDestGIdxList[j];
+ if ( gConnectOut[srcGIdx].count(destGIdx) > 0 )
+ costMatrix(i,j) = gConnectOut[srcGIdx][destGIdx]->cost;
+ else
+ costMatrix(i,j) = dbltype::infinity();
}
+ }
- //Add path to tracklet list
- gAssignedTracklets[newTrackletID].push_back(inEdges[destPtIdx][bestTrackletIdx]);
+ //dbgPrintCostMatrix("outcost.log", costMatrix, costSrcGIdxList, costDestGIdxList);
- //Keep track of assignment for fast lookup
- int srcGIdx = GetGlobalIdx(inEdges[destPtIdx][bestTrackletIdx]->frame[0], inEdges[destPtIdx][bestTrackletIdx]->index[0]);
- int destGIdx = GetGlobalIdx(t+1, destPtIdx);
- gAssignedConnectIn[destGIdx] = srcGIdx;
- gAssignedConnectOut[srcGIdx] = destGIdx;
+ Munkres hungarianSolver;
+ hungarianSolver.solve(costMatrix);
+
+ //dbgPrintCostMatrix("outassn.log", costMatrix, costSrcGIdxList, costDestGIdxList);
+
+ for ( int i=0; i < costSrcGIdxList.size(); ++i )
+ {
+ int srcGIdx = costSrcGIdxList[i];
+ for ( int j=0; j < costDestGIdxList.size(); ++j )
+ {
+ int destGIdx = costDestGIdxList[j];
+ // Edge is assigned
+ if ( costMatrix(i,j) != 0.0 )
+ continue;
+
+ // Edge exists in graph
+ if ( gConnectOut[srcGIdx].count(destGIdx) == 0 )
+ continue;
+
+ int newTrackletID = gConnectOut[srcGIdx][destGIdx]->trackletID;
+
+ if ( newTrackletID < 0 )
+ {
+ //Add new tracklet to list etc. and set id
+ newTrackletID = gAssignedTracklets.size();
+ gConnectOut[srcGIdx][destGIdx]->trackletID = newTrackletID;
+
+ tPathList newList;
+ gAssignedTracklets.push_back(newList);
+ }
+
+ //Add path to tracklet list
+ gAssignedTracklets[newTrackletID].push_back(gConnectOut[srcGIdx][destGIdx]);
+
+ //Keep track of assignment for fast lookup
+ gAssignedConnectIn[destGIdx] = srcGIdx;
+ gAssignedConnectOut[srcGIdx] = destGIdx;
+ }
}
}
diff --git a/src/tracker.h b/src/tracker.h
index 18e101e..bb936b4 100644
--- a/src/tracker.h
+++ b/src/tracker.h
@@ -3,6 +3,7 @@
#include <list>
#include <vector>
#include <limits>
+#include <set>
#include "detection.h"
#include "cost.h"
--
GitLab