const Tracks& NearestNeighborTracker::processCycle(double currentTime, const Observations& newObservations, const Observations& additionalLowConfidenceObservations)
{
beginCycle(currentTime);
predictTrackStates();
m_occlusionManager->manageOcclusionsBeforeDataAssociation(m_tracks, ros::Time(currentTime), m_frameID);
predictMeasurements();
Pairings pairings = performDataAssociation(m_tracks, newObservations);
// Extension to default NNT: If there are additional low-confidence observations (e.g. from a high-recall blob detector),
// try to match these against the tracks which are so far unmatched (but only if we are sufficiently sure that it is a valid track).
if(!additionalLowConfidenceObservations.empty())
{
int minNumMatchesToAllowLowConfidenceMatch = Params::get<int>("min_num_matches_to_allow_low_confidence_match", 10);
Tracks unmatchedTracks;
foreach(Track::Ptr track, m_tracks) {
if(track->trackStatus == Track::OCCLUDED && track->numberOfTotalMatches >= minNumMatchesToAllowLowConfidenceMatch) {
unmatchedTracks.push_back(track);
}
}
if(!unmatchedTracks.empty()) {
Pairings additionalPairings = performDataAssociation(unmatchedTracks, additionalLowConfidenceObservations);
pairings.insert(pairings.end(), additionalPairings.begin(), additionalPairings.end());
}
}
void GetTracksTabSeparated(string const& path, Tracks& tracks, vector<string>& names)
{
tracks.clear();
// Read the file to get our tracks
ifstream ifs(path);
string line;
int idx = 0;
while (getline(ifs, line)) {
stringstream ss(line);
string name;
getline(ss, name, '\t');
string key_str;
getline(ss, key_str, '\t');
Key key = Key::KeyFromString(key_str);
double bpm;
ss >> bpm;
// Allow commenting out tracks
if (!name.substr(0, 2).compare("//")) {
continue;
}
tracks.push_back(Track(idx++, bpm, key));
names.push_back(name);
}
ifs.close();
}
void ChooseTrack(
vector<string> names,
Tracks& available,
Tracks& chosen,
int prev_idx,
Key prev_key,
double prev_bpm,
double cost,
double& best_cost,
int max_len,
Tracks& best,
int& its
)
{
// We're too long!
if (chosen.size() >= static_cast<size_t>(max_len)) {
return;
}
// We have a new best length (more important than cost)
if (chosen.size() > best.size()) {
best_cost = cost;
best = chosen;
cout << "New best of length " << best.size() << " found." << endl;
its = 0;
}
// Mix length is the same, but less cost
else if (chosen.size() == best.size() && cost < best_cost && !chosen.empty()) {
best_cost = cost;
cout << "New best cost of " << best_cost << " found." << endl;
best = chosen;
its = 0;
}
// No improvement!
else {
++its;
}
// We've spent too long, so bail!
if (its >= 1000000) {
cout << "Too many iterations without improvement!" << endl;
return;
}
// Nothing to look at
if (available.empty()) {
cout << "No more tracks to choose." << endl;
return;
}
Track prev(prev_idx, prev_bpm, prev_key);
// Go through all available and find those compatible
Tracks candidates;
Keys candidates_adj;
vector<double> candidate_costs;
for (auto t : available) {
Key candidate_adj;
double this_cost;
if (AreCompatibleTracks(
prev,
t,
kBPMThresh,
kKeyShiftThresh,
candidate_adj,
this_cost
)) {
candidates.push_back(t);
candidates_adj.push_back(candidate_adj);
candidate_costs.push_back(this_cost);
}
}
for (size_t i = 0; i < candidates.size(); ++i) {
Tracks now_available = available;
Track t = candidates[i];
// Erase the candidate from the now available list
for (size_t j = 0; j < now_available.size(); ++j) {
if (now_available[j] == t) {
now_available.erase(now_available.begin() + j);
break;
}
}
// Set to our new key
t.key = candidates_adj[i];
Tracks now_chosen = chosen;
now_chosen.push_back(t);
// NOTE: We send in an ADJUSTED key for this track!
ChooseTrack(names, now_available, now_chosen, t.idx, t.key, t.bpm, cost + candidate_costs[i], best_cost, max_len, best, its);
}
}
