forked from Smorodov/Multitarget-tracker
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathShortPathCalculator.cpp
More file actions
64 lines (55 loc) · 1.55 KB
/
ShortPathCalculator.cpp
File metadata and controls
64 lines (55 loc) · 1.55 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
#include "ShortPathCalculator.h"
#include <GTL/GTL.h>
#include "mygraph.h"
#include "mwbmatching.h"
#include "tokenise.h"
///
/// \brief SPBipart::Solve
/// \param costMatrix
/// \param N
/// \param M
/// \param assignment
/// \param maxCost
///
void SPBipart::Solve(const distMatrix_t& costMatrix, size_t N, size_t M, assignments_t& assignment, track_t maxCost)
{
MyGraph G;
G.make_directed();
std::vector<GTL::node> nodes(N + M);
for (size_t i = 0; i < nodes.size(); ++i)
{
nodes[i] = G.new_node();
}
GTL::edge_map<int> weights(G, 100);
for (size_t i = 0; i < N; i++)
{
bool hasZeroEdge = false;
for (size_t j = 0; j < M; j++)
{
track_t currCost = costMatrix[i + j * N];
GTL::edge e = G.new_edge(nodes[i], nodes[N + j]);
if (currCost < m_settings.m_distThres)
{
int weight = static_cast<int>(maxCost - currCost + 1);
G.set_edge_weight(e, weight);
weights[e] = weight;
}
else
{
if (!hasZeroEdge)
{
G.set_edge_weight(e, 0);
weights[e] = 0;
}
hasZeroEdge = true;
}
}
}
GTL::edges_t L = MAX_WEIGHT_BIPARTITE_MATCHING(G, weights);
for (GTL::edges_t::iterator it = L.begin(); it != L.end(); ++it)
{
GTL::node a = it->source();
GTL::node b = it->target();
assignment[b.id()] = static_cast<assignments_t::value_type>(a.id() - N);
}
}