//=============================================================================
// Main execution
//=============================================================================
StatusCode TbTracking::execute() {
for (unsigned int i = 0; i < m_nPlanes; ++i) {
const std::string clusterLocation = m_clusterLocation + std::to_string(i);
LHCb::TbClusters* clusters = getIfExists<LHCb::TbClusters>(clusterLocation);
if (!clusters) {
error() << "No clusters in " << clusterLocation << endmsg;
return StatusCode::FAILURE;
}
// Store the cluster iterators in the cluster finder.
m_clusterFinder->setClusters(clusters, i);
}
// Clear Kalman track container
ktracks_vec.clear();
// Create a track container and transfer its ownership to the TES.
m_tracks = new LHCb::TbTracks();
put(m_tracks, LHCb::TbTrackLocation::Default);
// Do the tracking and time order.
performTracking();
timeOrderTracks();
// fill the histos with the Kalman fit results
fill_khists(ktracks_vec);
counter("NumberOfTracks") += m_tracks->size();
return StatusCode::SUCCESS;
}
void performTracking(const PointCloudConstPtr& cloud)
{
typename pcl::search::KdTree<PointType>::Ptr search(
new pcl::search::KdTree<PointType>());
search->setInputCloud(cloud);
performTracking(cloud, search);
}
void PTracker::run()
{
IdGenerator idGenerator(0);
frameCount = 0;
while(true)
{
SubFrame subResult = receive(subtractorBuffer);
ClasifierFrame detectionResult = receive(classifierBuffer);
bool buffersFull = shiftBuffers(subResult, detectionResult);
if(!buffersFull)
continue;
frameCount++;
#if PROCESS
#pragma region setting data for time t
auto detections = detectionBuffer[1];
auto currentFrame = frameBuffer[1];
auto currentGrayFrame = grayFrameBuffer[1];
auto prevFrame = frameBuffer[0];
auto currentForeground = foregroundBuffer[1];
#pragma endregion
#pragma region init tracks from first detections
if(tracks.size() == 0 && detections.size() > 0)
{
tracks.clear();
tracks.reserve(detections.size());
for_each(begin(detections), end(detections), [&](detection& d){
createTrack(d, idGenerator, currentGrayFrame);
});
continue;
}
#pragma endregion
#pragma region predict track positions for time t
auto lkoutput = currentFrame.clone();
deleteExitedTracks();
beginTracking();
for(auto it = begin(tracks); it != end(tracks); it++)
registerForTracking(*it);
performTracking();
for(auto it = begin(tracks); it != end(tracks); it++)
{
Rect kanadePrediction, kalmanPrediction;
bool lucasSuccess = getMedianFlowPrediction(*it, kanadePrediction);
bool kalmanSuccess = getKalmanPrediction(*it, kalmanPrediction);
float minDist = 999999;
auto finalPrediction = mergePredictions(lucasSuccess, kalmanSuccess, *it, kanadePrediction, kalmanPrediction, grayFrameBuffer, minDist);
it->assign(finalPrediction);
it->predictionDist = minDist;
}
#pragma endregion
std::map<int, trackMatch> trMatches;
std::map<int, detectionMatch> detMatches;
matcher->begin();
secMatcher->begin();
for_each(begin(tracks), end(tracks), [&](track& tr){
auto dit = detections.begin();
auto dend = detections.end();
for(;dit != dend; ++dit)
{
float score = matcher->match(tr, *dit, currentGrayFrame);
if(score < 0)
continue;
float dist = secMatcher->match(tr, *dit, currentGrayFrame);
if(dist > secMatcher->goodMaxDist)
{
continue;
}
trackMatch trMatch = {dit->id, score};
detectionMatch dMatch = {tr.id, score};
if(detMatches.find(dit->id) == detMatches.end())
{
trMatches[tr.id] = trMatch;
detMatches[dit->id] = dMatch;
}else if(detMatches[dit->id].score < score)
{
auto exmatch = detMatches[dit->id];
trMatches.erase(exmatch.trackId);
trMatches[tr.id] = trMatch;
detMatches[dit->id] = dMatch;
}
}
});
#pragma region update track models and kalman
for_each(begin(tracks), end(tracks), [&](track& tr){
if(trMatches.find(tr.id) != trMatches.end())//matched detection with track
{
auto m = trMatches[tr.id];
matcher->inferModel(tr, detections[m.detectionId], currentGrayFrame);
secMatcher->inferModel(tr, detections[m.detectionId], currentGrayFrame);
validators[tr.id].tick(true);
correctKalman(tr);
}else
{//detection not found for track
float max = secMatcher->maxSimilarityDist;
float dist = tr.predictionDist;
if(dist < max)
validators[tr.id].tick(true);
else
validators[tr.id].tick(false);
forwardKalman(tr);
}
});
#pragma endregion
#pragma region init new tracks from unmatched detections
for_each(begin(detections), end(detections), [&](detection& mockdet){
auto it = detMatches.find(mockdet.id);
if(it == detMatches.end()){
auto inited = createTrack(mockdet, idGenerator, currentGrayFrame);
trackMatch m = {mockdet.id, 1};
trMatches[inited.id] = m;
}
});
#pragma endregion
#pragma region drawing_results
if(debugPrint)
{
printf("frame %d========\n", frameCount);
for(auto it = begin(tracks); it != end(tracks); it++)
{
if(trMatches.find(it->id) != trMatches.end())
printf("track %d : detection %d\n", it->id, trMatches[it->id].detectionId);
}
cv::waitKey();
}
auto fclone = currentFrame.clone();
DrawExtensions::drawDetections(detections, fclone);
DrawExtensions::drawTracks(tracks, fclone, Scalar(255,0,0));
for_each(begin(tracks), end(tracks), [&](track& tr){
Draw::rect(tr.model.kalmanRect, fclone, Scalar(0,0,255));
});
//imshow("xxx", currentForeground);
//cv::waitKey(50);
std::stringstream str;
str << carCount;
Draw::text(str.str(), Point(10,20), fclone, Scalar(255,255,0));
imshow("kalman", fclone);
fclone.release();
lkoutput.release();
#pragma endregion
#endif
char key;
key = cv::waitKey(1.);
if(key == 's')
debugPrint = true;
else if(key == 'f')
debugPrint = false;
send(syncBuffer,1);
}
}
