344 lines
9.2 KiB
C
344 lines
9.2 KiB
C
|
|
||
|
|
/* Header File Including ------------------------------------------------------------------------ */#include <stdint.h>
|
||
|
|
#include <string.h>
|
||
|
|
#include "m_munkres.h"
|
||
|
|
|
||
|
|
//#include "loc_vivo_pdr.h"
|
||
|
|
/* Macro Definition ----------------------------------------------------------------------------- */
|
||
|
|
#define COST(i, j) cost[(i) + (j) * m]
|
||
|
|
#define MARK(i, j) mark[(i) + (j) * m]
|
||
|
|
#define PATH(i, j) path[(i) + (j) * 64]
|
||
|
|
#define CLR_ALL(flag) (*flag##_cover = 0)
|
||
|
|
#define SET_FLG(flag, index) (*flag##_cover |= (1 << (index)))
|
||
|
|
#define CLR_FLG(flag, index) (*flag##_cover &= ~(1 << (index)))
|
||
|
|
#define TST_FLG(flag, index) (*flag##_cover & (1 << (index)))
|
||
|
|
|
||
|
|
/* Global Variable Definition ------------------------------------------------------------------- */
|
||
|
|
static float g_cost[64];
|
||
|
|
static int g_mark[64];
|
||
|
|
static uint8_t g_row_cover;
|
||
|
|
static uint8_t g_col_cover;
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
/* Function Declaration ------------------------------------------------------------------------- */
|
||
|
|
static int step_one(float *cost, int m, int n);
|
||
|
|
static int step_two(float *cost, int *mark, int m, int n, uint8_t *col_cover);
|
||
|
|
static int step_three(int *mark, int m, int n, uint8_t *col_cover);
|
||
|
|
static int step_four(float *cost, int *mark, int m, int n, uint8_t *row_cover, uint8_t *col_cover, int *path_start_i, int *path_start_j);
|
||
|
|
static int step_five(int *mark, int m, int n, uint8_t *row_cover, uint8_t *col_cover, int path_start_i, int path_start_j);
|
||
|
|
static int step_six(float *cost, int m, int n, uint8_t *row_cover, uint8_t *col_cover);
|
||
|
|
|
||
|
|
#ifdef __MATLAB_DBG
|
||
|
|
void display_cost_matrix_and_cover(float *cost, int m, int n, uint8_t row_cover, uint8_t col_cover);
|
||
|
|
void display_mark_matrix_and_cover(int *mark, int m, int n, uint8_t row_cover, uint8_t col_cover);
|
||
|
|
#endif
|
||
|
|
|
||
|
|
/* Function Definition -------------------------------------------------------------------------- */
|
||
|
|
int MUNKRES_get_assignment(int *assignment, float *cost, int m, int n) {
|
||
|
|
int i = 0;
|
||
|
|
int j = 0;
|
||
|
|
int step;
|
||
|
|
|
||
|
|
#if PRINT_ALGO_RUN_TIME
|
||
|
|
uint64 time_algo_1 = 0l;
|
||
|
|
uint64 time_algo_2 = 0l;
|
||
|
|
uint64 time_algo_3 = 0l;
|
||
|
|
uint64 time_algo_4 = 0l;
|
||
|
|
|
||
|
|
#endif
|
||
|
|
|
||
|
|
if (m > 8 || n > 8) {
|
||
|
|
return MUNKRES_EXCESSIVE_MATRIX;
|
||
|
|
}
|
||
|
|
|
||
|
|
memcpy(g_cost, cost, m * n * sizeof(*g_cost));
|
||
|
|
memset(g_mark, 0, m * n * sizeof(*g_mark));
|
||
|
|
g_row_cover = 0;
|
||
|
|
g_col_cover = 0;
|
||
|
|
|
||
|
|
step = 0;
|
||
|
|
#if PRINT_ALGO_RUN_TIME
|
||
|
|
time_algo_1 = uTimetick_Get();
|
||
|
|
#endif
|
||
|
|
while (7 != step) {
|
||
|
|
#ifdef __MATLAB_DBG
|
||
|
|
mexPrintf("step %d:\n", step);
|
||
|
|
#endif
|
||
|
|
switch (step) {
|
||
|
|
case 0:
|
||
|
|
case 1:
|
||
|
|
step = step_one(g_cost, m, n);
|
||
|
|
break;
|
||
|
|
case 2:
|
||
|
|
step = step_two(g_cost, g_mark, m, n, &g_col_cover);
|
||
|
|
break;
|
||
|
|
case 3:
|
||
|
|
step = step_three(g_mark, m, n, &g_col_cover);
|
||
|
|
break;
|
||
|
|
case 4:
|
||
|
|
step = step_four(g_cost, g_mark, m, n, &g_row_cover, &g_col_cover, &i, &j);
|
||
|
|
#if PRINT_ALGO_RUN_TIME
|
||
|
|
time_algo_3 = uTimetick_Get();
|
||
|
|
time_algo_3 = (time_algo_3-time_algo_1);
|
||
|
|
|
||
|
|
#if defined(LOCATION_PLATFORM_QCOM_MODEM)
|
||
|
|
Geek_PDR_LOGH_NMEA("step_4 run time ms %d",(int)time_algo_3,0,0,0);
|
||
|
|
#endif
|
||
|
|
#endif
|
||
|
|
break;
|
||
|
|
case 5:
|
||
|
|
step = step_five(g_mark, m, n, &g_row_cover, &g_col_cover, i, j);
|
||
|
|
#if PRINT_ALGO_RUN_TIME
|
||
|
|
time_algo_4 = uTimetick_Get();
|
||
|
|
time_algo_4 = (time_algo_4-time_algo_1);
|
||
|
|
|
||
|
|
#if defined(LOCATION_PLATFORM_QCOM_MODEM)
|
||
|
|
Geek_PDR_LOGH_NMEA("step_5 run time ms %d",(int)time_algo_4,0,0,0);
|
||
|
|
#endif
|
||
|
|
#endif
|
||
|
|
break;
|
||
|
|
case 6:
|
||
|
|
step = step_six(g_cost, m, n, &g_row_cover, &g_col_cover);
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
step = 7;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
#ifdef __MATLAB_DBG
|
||
|
|
mexPrintf(" cost:\n", step);
|
||
|
|
display_cost_matrix_and_cover(g_cost, m, n, g_row_cover, g_col_cover);
|
||
|
|
mexPrintf(" mark:\n", step);
|
||
|
|
display_mark_matrix_and_cover(g_mark, m, n, g_row_cover, g_col_cover);
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
#if PRINT_ALGO_RUN_TIME
|
||
|
|
time_algo_2 = uTimetick_Get();
|
||
|
|
#endif
|
||
|
|
#if PRINT_ALGO_RUN_TIME
|
||
|
|
|
||
|
|
time_algo_2 = (time_algo_2-time_algo_1);
|
||
|
|
// time_algo_3 = (time_algo_3-time_algo_2);
|
||
|
|
|
||
|
|
#if defined(LOCATION_PLATFORM_QCOM_MODEM)
|
||
|
|
Geek_PDR_LOGH_NMEA("munkres run time ms %d",(int)time_algo_2,0,0,0);
|
||
|
|
#endif
|
||
|
|
// Geek_PDR_LOGH_NMEA("pdr2 run time ms %d %d",(int)time_algo_5,time_algo_6,0,0);
|
||
|
|
#endif
|
||
|
|
|
||
|
|
for (i = 0; i < m; ++i) {
|
||
|
|
for (j = 0; j < n; ++j) {
|
||
|
|
if (0 != g_mark[(i) + (j) * m]) {
|
||
|
|
assignment[i] = j;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
return MUNKRES_NO_ERROR;
|
||
|
|
}
|
||
|
|
|
||
|
|
static int step_one(float *cost, int m, int n) {
|
||
|
|
int i;
|
||
|
|
int j;
|
||
|
|
float min;
|
||
|
|
for (i = 0; i < m; ++i) {
|
||
|
|
min = COST(i, 0);
|
||
|
|
for (j = 1; j < n; ++j) {
|
||
|
|
if (min > COST(i, j)) {
|
||
|
|
min = COST(i, j);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
for (j = 0; j < n; ++j) {
|
||
|
|
COST(i, j) -= min;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
return 2;
|
||
|
|
}
|
||
|
|
|
||
|
|
static int step_two(float *cost, int *mark, int m, int n, uint8_t *col_cover) {
|
||
|
|
int i;
|
||
|
|
int j;
|
||
|
|
|
||
|
|
for (i = 0; i < m; ++i) {
|
||
|
|
for (j = 0; j < n; ++j) {
|
||
|
|
if (0 == COST(i, j) && 0 == TST_FLG(col, j)) {
|
||
|
|
MARK(i, j) = 1;
|
||
|
|
SET_FLG(col, j);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
CLR_ALL(col);
|
||
|
|
|
||
|
|
return 3;
|
||
|
|
}
|
||
|
|
|
||
|
|
static int step_three(int *mark, int m, int n, uint8_t *col_cover) {
|
||
|
|
int i;
|
||
|
|
int j;
|
||
|
|
int count;
|
||
|
|
|
||
|
|
CLR_ALL(col);
|
||
|
|
for (i = 0; i < m; ++i) {
|
||
|
|
for (j = 0; j < n; ++j) {
|
||
|
|
if (1 == MARK(i, j)) {
|
||
|
|
SET_FLG(col, j);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
count = 0;
|
||
|
|
for (j = 0; j < n; ++j) {
|
||
|
|
count += (0 != TST_FLG(col, j) ? 1 : 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
return (count >= m || count >= n) ? 7 : 4;
|
||
|
|
}
|
||
|
|
|
||
|
|
static void find_a_noncovered_zero(int *i, int *j, float *cost, int m, int n, uint8_t *row_cover, uint8_t *col_cover) {
|
||
|
|
for (*i = 0; *i < m; ++*i) {
|
||
|
|
if (0 != TST_FLG(row, *i)) {
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
for (*j = 0; *j < n; ++*j) {
|
||
|
|
if (0 == COST(*i, *j) && 0 == TST_FLG(col, *j)) {
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (n != *j) {
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static void find_a_star_zero_in_row(int *j, int i, int *mark, int m, int n) {
|
||
|
|
for (*j = 0; *j < n; ++*j) {
|
||
|
|
if (1 == MARK(i, *j)) {
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static int step_four(float *cost, int *mark, int m, int n, uint8_t *row_cover, uint8_t *col_cover, int *path_start_i, int *path_start_j) {
|
||
|
|
int i;
|
||
|
|
int j;
|
||
|
|
int jj;
|
||
|
|
while (1) {
|
||
|
|
find_a_noncovered_zero(&i, &j, cost, m, n, row_cover, col_cover);
|
||
|
|
if (m == i) {
|
||
|
|
return 6;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
MARK(i, j) = 2;
|
||
|
|
find_a_star_zero_in_row(&jj, i, mark, m, n);
|
||
|
|
if (m != jj) {
|
||
|
|
SET_FLG(row, i);
|
||
|
|
CLR_FLG(col, jj);
|
||
|
|
j = jj;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
*path_start_i = i;
|
||
|
|
*path_start_j = j;
|
||
|
|
return 5;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static void find_a_star_zero_in_col(int *i, int j, int *mark, int m) {
|
||
|
|
for (*i = 0; *i < m; ++*i) {
|
||
|
|
if (1 == MARK(*i, j)) {
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static void find_a_prime_zero_in_row(int *j, int i, int *mark, int m, int n) {
|
||
|
|
for (*j = 0; *j < n; ++*j) {
|
||
|
|
if (2 == MARK(i, *j)) {
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static int step_five(int *mark, int m, int n, uint8_t *row_cover, uint8_t *col_cover, int path_start_i, int path_start_j) {
|
||
|
|
static int path[64 * 2];
|
||
|
|
int i;
|
||
|
|
int j;
|
||
|
|
int k;
|
||
|
|
|
||
|
|
k = 0;
|
||
|
|
PATH(k, 0) = path_start_i;
|
||
|
|
PATH(k, 1) = path_start_j;
|
||
|
|
while (1) {
|
||
|
|
find_a_star_zero_in_col(&i, PATH(k, 1), mark, m);
|
||
|
|
if (m == i) {
|
||
|
|
goto STEP_FIVE_EXIT;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
PATH(k + 1, 0) = i;
|
||
|
|
PATH(k + 1, 1) = PATH(k, 1);
|
||
|
|
++k;
|
||
|
|
find_a_prime_zero_in_row(&j, PATH(k, 0), mark, m, n);
|
||
|
|
PATH(k + 1, 0) = PATH(k, 0);
|
||
|
|
PATH(k + 1, 1) = j;
|
||
|
|
++k;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
STEP_FIVE_EXIT:
|
||
|
|
/* augment path */
|
||
|
|
for(; k >= 0; --k) {
|
||
|
|
MARK(PATH(k, 0), PATH(k, 1)) = (1 == MARK(PATH(k, 0), PATH(k, 1)) ? 0 : 1);
|
||
|
|
}
|
||
|
|
/* clear covers */
|
||
|
|
CLR_ALL(row);
|
||
|
|
CLR_ALL(col);
|
||
|
|
/* erease primes */
|
||
|
|
for (i = 0; i < m; ++i) {
|
||
|
|
for (j = 0; j < n; ++j) {
|
||
|
|
if (2 == MARK(i, j)) {
|
||
|
|
MARK(i, j) = 0;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
return 3;
|
||
|
|
}
|
||
|
|
|
||
|
|
static int step_six(float *cost, int m, int n, uint8_t *row_cover, uint8_t *col_cover) {
|
||
|
|
float min;
|
||
|
|
int i;
|
||
|
|
int j;
|
||
|
|
|
||
|
|
min = 1e6;
|
||
|
|
for (i = 0; i < m; ++i) {
|
||
|
|
for (j = 0; j < n; ++j) {
|
||
|
|
if (0 == TST_FLG(row, i) && 0 == TST_FLG(col, j)) {
|
||
|
|
if (min > COST(i, j)) {
|
||
|
|
min = COST(i, j);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
for (i = 0; i < m; ++i) {
|
||
|
|
for (j = 0; j < n; ++j) {
|
||
|
|
if (0 != TST_FLG(row, i)) {
|
||
|
|
COST(i, j) += min;
|
||
|
|
}
|
||
|
|
if (0 == TST_FLG(col, j)) {
|
||
|
|
COST(i, j) -= min;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
return 4;
|
||
|
|
}
|
||
|
|
|
||
|
|
|