368 lines
9.5 KiB
Text
368 lines
9.5 KiB
Text
/*
|
|
* lap implementation
|
|
*
|
|
* Copyright (C) YuanJun, ZhangLiang, Li Wei, Li Ming, Andreas Nuechter,
|
|
*
|
|
* Released under the GPL version 3.
|
|
*
|
|
*/
|
|
|
|
/**
|
|
* @file
|
|
* @brief
|
|
*
|
|
* @author Andreas Nuechter. Jacobs University Bremen, Germany
|
|
* @author YuanJun, Wuhan University, China
|
|
* @author ZhangLiang, Wuhan University, China
|
|
* @author Li Wei, Wuhan University, China
|
|
* @author Li Ming, Wuhan University, China
|
|
*/
|
|
|
|
#include "veloslam/lap.h"
|
|
|
|
cost lap(int dim,
|
|
cost **assigncost,
|
|
col *rowsol,
|
|
row *colsol,
|
|
cost *u,
|
|
cost *v)
|
|
|
|
// input:
|
|
// dim - problem size
|
|
// assigncost - cost matrix
|
|
|
|
// output:
|
|
// rowsol - column assigned to row in solution
|
|
// colsol - row assigned to column in solution
|
|
// u - dual variables, row reduction numbers
|
|
// v - dual variables, column reduction numbers
|
|
|
|
{
|
|
Boolean unassignedfound;
|
|
row i, imin, numfree = 0, prvnumfree, f, i0, k, freerow, *pred, *free;
|
|
col j, j1, j2, endofpath, last, low, up, *collist, *matches;
|
|
cost min, h, umin, usubmin, v2, *d;
|
|
|
|
free = new row[dim]; // list of unassigned rows.
|
|
collist = new col[dim]; // list of columns to be scanned in various ways.
|
|
matches = new col[dim]; // counts how many times a row could be assigned.
|
|
d = new cost[dim]; // 'cost-distance' in augmenting path calculation.
|
|
pred = new row[dim]; // row-predecessor of column in augmenting/alternating path.
|
|
|
|
// init how many times a row will be assigned in the column reduction.
|
|
for (i = 0; i < dim; i++)
|
|
matches[i] = 0;
|
|
|
|
// COLUMN REDUCTION
|
|
for (j = dim-1; j >= 0; j--) // reverse order gives better results.
|
|
{
|
|
// find minimum cost over rows.
|
|
min = assigncost[0][j];
|
|
imin = 0;
|
|
for (i = 1; i < dim; i++)
|
|
if (assigncost[i][j] < min)
|
|
{
|
|
min = assigncost[i][j];
|
|
imin = i;
|
|
}
|
|
v[j] = min;
|
|
|
|
if (++matches[imin] == 1)
|
|
{
|
|
// init assignment if minimum row assigned for first time.
|
|
rowsol[imin] = j;
|
|
colsol[j] = imin;
|
|
}
|
|
else
|
|
colsol[j] = -1; // row already assigned, column not assigned.
|
|
}
|
|
|
|
// REDUCTION TRANSFER
|
|
for (i = 0; i < dim; i++)
|
|
if (matches[i] == 0) // fill list of unassigned 'free' rows.
|
|
free[numfree++] = i;
|
|
else
|
|
if (matches[i] == 1) // transfer reduction from rows that are assigned once.
|
|
{
|
|
j1 = rowsol[i];
|
|
min = BIG;
|
|
for (j = 0; j < dim; j++)
|
|
if (j != j1)
|
|
if (assigncost[i][j] - v[j] < min)
|
|
min = assigncost[i][j] - v[j];
|
|
v[j1] = v[j1] - min;
|
|
}
|
|
|
|
// AUGMENTING ROW REDUCTION
|
|
int loopcnt = 0; // do-loop to be done twice.
|
|
do
|
|
{
|
|
loopcnt++;
|
|
|
|
// scan all free rows.
|
|
// in some cases, a free row may be replaced with another one to be scanned next.
|
|
k = 0;
|
|
prvnumfree = numfree;
|
|
numfree = 0; // start list of rows still free after augmenting row reduction.
|
|
while (k < prvnumfree)
|
|
{
|
|
i = free[k];
|
|
k++;
|
|
|
|
// find minimum and second minimum reduced cost over columns.
|
|
umin = assigncost[i][0] - v[0];
|
|
j1 = 0;
|
|
usubmin = BIG;
|
|
for (j = 1; j < dim; j++)
|
|
{
|
|
h = assigncost[i][j] - v[j];
|
|
if (h < usubmin)
|
|
if (h >= umin)
|
|
{
|
|
usubmin = h;
|
|
j2 = j;
|
|
}
|
|
else
|
|
{
|
|
usubmin = umin;
|
|
umin = h;
|
|
j2 = j1;
|
|
j1 = j;
|
|
}
|
|
}
|
|
|
|
i0 = colsol[j1];
|
|
if (umin < usubmin)
|
|
// change the reduction of the minimum column to increase the minimum
|
|
// reduced cost in the row to the subminimum.
|
|
v[j1] = v[j1] - (usubmin - umin);
|
|
else // minimum and subminimum equal.
|
|
if (i0 >= 0) // minimum column j1 is assigned.
|
|
{
|
|
// swap columns j1 and j2, as j2 may be unassigned.
|
|
j1 = j2;
|
|
i0 = colsol[j2];
|
|
}
|
|
|
|
// (re-)assign i to j1, possibly de-assigning an i0.
|
|
rowsol[i] = j1;
|
|
colsol[j1] = i;
|
|
|
|
if (i0 >= 0) // minimum column j1 assigned earlier.
|
|
if (umin < usubmin)
|
|
// put in current k, and go back to that k.
|
|
// continue augmenting path i - j1 with i0.
|
|
free[--k] = i0;
|
|
else
|
|
// no further augmenting reduction possible.
|
|
// store i0 in list of free rows for next phase.
|
|
free[numfree++] = i0;
|
|
}
|
|
}
|
|
while (loopcnt < 2); // repeat once.
|
|
|
|
// AUGMENT SOLUTION for each free row.
|
|
for (f = 0; f < numfree; f++)
|
|
{
|
|
freerow = free[f]; // start row of augmenting path.
|
|
|
|
// Dijkstra shortest path algorithm.
|
|
// runs until unassigned column added to shortest path tree.
|
|
for (j = 0; j < dim; j++)
|
|
{
|
|
d[j] = assigncost[freerow][j] - v[j];
|
|
pred[j] = freerow;
|
|
collist[j] = j; // init column list.
|
|
}
|
|
|
|
low = 0; // columns in 0..low-1 are ready, now none.
|
|
up = 0; // columns in low..up-1 are to be scanned for current minimum, now none.
|
|
// columns in up..dim-1 are to be considered later to find new minimum,
|
|
// at this stage the list simply contains all columns
|
|
unassignedfound = FALSE;
|
|
do
|
|
{
|
|
if (up == low) // no more columns to be scanned for current minimum.
|
|
{
|
|
last = low - 1;
|
|
|
|
// scan columns for up..dim-1 to find all indices for which new minimum occurs.
|
|
// store these indices between low..up-1 (increasing up).
|
|
min = d[collist[up++]];
|
|
for (k = up; k < dim; k++)
|
|
{
|
|
j = collist[k];
|
|
h = d[j];
|
|
if (h <= min)
|
|
{
|
|
if (h < min) // new minimum.
|
|
{
|
|
up = low; // restart list at index low.
|
|
min = h;
|
|
}
|
|
// new index with same minimum, put on undex up, and extend list.
|
|
collist[k] = collist[up];
|
|
collist[up++] = j;
|
|
}
|
|
}
|
|
|
|
// check if any of the minimum columns happens to be unassigned.
|
|
// if so, we have an augmenting path right away.
|
|
for (k = low; k < up; k++)
|
|
if (colsol[collist[k]] < 0)
|
|
{
|
|
endofpath = collist[k];
|
|
unassignedfound = TRUE;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (!unassignedfound)
|
|
{
|
|
// update 'distances' between freerow and all unscanned columns, via next scanned column.
|
|
j1 = collist[low];
|
|
low++;
|
|
i = colsol[j1];
|
|
h = assigncost[i][j1] - v[j1] - min;
|
|
|
|
for (k = up; k < dim; k++)
|
|
{
|
|
j = collist[k];
|
|
v2 = assigncost[i][j] - v[j] - h;
|
|
if (v2 < d[j])
|
|
{
|
|
pred[j] = i;
|
|
if (v2 == min) // new column found at same minimum value
|
|
if (colsol[j] < 0)
|
|
{
|
|
// if unassigned, shortest augmenting path is complete.
|
|
endofpath = j;
|
|
unassignedfound = TRUE;
|
|
break;
|
|
}
|
|
// else add to list to be scanned right away.
|
|
else
|
|
{
|
|
collist[k] = collist[up];
|
|
collist[up++] = j;
|
|
}
|
|
d[j] = v2;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
while (!unassignedfound);
|
|
|
|
// update column prices.
|
|
for (k = 0; k <= last; k++)
|
|
{
|
|
j1 = collist[k];
|
|
v[j1] = v[j1] + d[j1] - min;
|
|
}
|
|
|
|
// reset row and column assignments along the alternating path.
|
|
do
|
|
{
|
|
i = pred[endofpath];
|
|
colsol[endofpath] = i;
|
|
j1 = endofpath;
|
|
endofpath = rowsol[i];
|
|
rowsol[i] = j1;
|
|
}
|
|
while (i != freerow);
|
|
}
|
|
|
|
// calculate optimal cost.
|
|
cost lapcost = 0;
|
|
for (i = 0; i < dim; i++)
|
|
{
|
|
j = rowsol[i];
|
|
u[i] = assigncost[i][j] - v[j];
|
|
lapcost = lapcost + assigncost[i][j];
|
|
}
|
|
|
|
// free reserved memory.
|
|
delete[] pred;
|
|
delete[] free;
|
|
delete[] collist;
|
|
delete[] matches;
|
|
delete[] d;
|
|
|
|
return lapcost;
|
|
}
|
|
|
|
void checklap(int dim,
|
|
cost **assigncost,
|
|
col *rowsol,
|
|
row *colsol,
|
|
cost *u,
|
|
cost *v)
|
|
{
|
|
row i;
|
|
col j;
|
|
cost lapcost = 0, redcost = 0;
|
|
Boolean *matched;
|
|
char wait;
|
|
|
|
matched = new Boolean[dim];
|
|
|
|
for (i = 0; i < dim; i++)
|
|
for (j = 0; j < dim; j++)
|
|
if ((redcost = assigncost[i][j] - u[i] - v[j]) < 0)
|
|
{
|
|
printf("\n");
|
|
printf("negative reduced cost i %d j %d redcost %d\n", i, j, redcost);
|
|
printf("\n\ndim %5d - press key\n", dim);
|
|
scanf("%d", &wait);
|
|
break;
|
|
}
|
|
|
|
for (i = 0; i < dim; i++)
|
|
if ((redcost = assigncost[i][rowsol[i]] - u[i] - v[rowsol[i]]) != 0)
|
|
{
|
|
printf("\n");
|
|
printf("non-null reduced cost i %d soli %d redcost %d\n", i, rowsol[i], redcost);
|
|
printf("\n\ndim %5d - press key\n", dim);
|
|
scanf("%d", &wait);
|
|
break;
|
|
}
|
|
|
|
for (j = 0; j < dim; j++)
|
|
matched[j] = FALSE;
|
|
|
|
for (i = 0; i < dim; i++)
|
|
if (matched[rowsol[i]])
|
|
{
|
|
printf("\n");
|
|
printf("column matched more than once - i %d soli %d\n", i, rowsol[i]);
|
|
printf("\n\ndim %5d - press key\n", dim);
|
|
scanf("%d", &wait);
|
|
break;
|
|
}
|
|
else
|
|
matched[rowsol[i]] = TRUE;
|
|
|
|
|
|
for (i = 0; i < dim; i++)
|
|
if (colsol[rowsol[i]] != i)
|
|
{
|
|
printf("\n");
|
|
printf("error in row solution i %d soli %d solsoli %d\n", i, rowsol[i], colsol[rowsol[i]]);
|
|
printf("\n\ndim %5d - press key\n", dim);
|
|
scanf("%d", &wait);
|
|
break;
|
|
}
|
|
|
|
for (j = 0; j < dim; j++)
|
|
if (rowsol[colsol[j]] != j)
|
|
{
|
|
printf("\n");
|
|
printf("error in col solution j %d solj %d solsolj %d\n", j, colsol[j], rowsol[colsol[j]]);
|
|
printf("\n\ndim %5d - press key\n", dim);
|
|
scanf("%d", &wait);
|
|
break;
|
|
}
|
|
|
|
delete[] matched;
|
|
return;
|
|
}
|