// ---------------------------------------------------------------------------
//
// ---------------------------------------------------------------------------
void CTracker::Update(vector<Point2f>& detections)
{
// -----------------------------------
// If there is no tracks yet, then every point begins its own track.
// -----------------------------------
if(tracks.size()==0)
{
// If no tracks yet
for(int i=0;i<detections.size();i++)
{
CTrack* tr=new CTrack(detections[i],dt,Accel_noise_mag);
tracks.push_back(tr);
}
}
// -----------------------------------
// «десь треки уже есть в любом случае
// -----------------------------------
int N=tracks.size(); // треки
int M=detections.size(); // детекты
// ћатрица рассто¤ний от N-ного трека до M-ного детекта.
vector< vector<double> > Cost(N,vector<double>(M));
vector<int> assignment; // назначени¤
// -----------------------------------
// “реки уже есть, составим матрицу рассто¤ний
// -----------------------------------
double dist;
for(int i=0;i<tracks.size();i++)
{
Point2d prediction=tracks[i]->prediction;
for(int j=0;j<detections.size();j++)
{
//cal prediction and detection
Point2d diff=(tracks[i]->prediction-detections[j]);
dist=sqrtf(diff.x*diff.x+diff.y*diff.y);
Cost[i][j]=dist;
}
}
// -----------------------------------
// Solving assignment problem (tracks and predictions of Kalman filter)
// -----------------------------------
AssignmentProblemSolver APS;
APS.Solve(Cost,assignment,AssignmentProblemSolver::optimal);
// -----------------------------------
// clean assignment from pairs with large distance
// -----------------------------------
// Not assigned tracks
vector<int> not_assigned_tracks;
for(int i=0;i<assignment.size();i++)
{
if(assignment[i]!=-1)
{
if(Cost[i][assignment[i]]>dist_thres)
{
assignment[i]=-1;
// Mark unassigned tracks, and increment skipped frames counter,
// when skipped frames counter will be larger than threshold, track will be deleted.
not_assigned_tracks.push_back(i);
}
}
else
{
// If track have no assigned detect, then increment skipped frames counter.
tracks[i]->skipped_frames++;
}
}
// -----------------------------------
// If track didn't get detects long time, remove it.
// -----------------------------------
for(int i=0;i<tracks.size();i++)
{
if(tracks[i]->skipped_frames>maximum_allowed_skipped_frames)
{
delete tracks[i];
tracks.erase(tracks.begin()+i);
assignment.erase(assignment.begin()+i);
i--;
}
}
// -----------------------------------
// Search for unassigned detects
// -----------------------------------
vector<int> not_assigned_detections;
vector<int>::iterator it;
for(int i=0;i<detections.size();i++)
{
it=find(assignment.begin(), assignment.end(), i);
if(it==assignment.end())
{
not_assigned_detections.push_back(i);
}
}
// -----------------------------------
// and start new tracks for them.
// -----------------------------------
if(not_assigned_detections.size()!=0)
{
for(int i=0;i<not_assigned_detections.size();i++)
{
CTrack* tr=new CTrack(detections[not_assigned_detections[i]],dt,Accel_noise_mag);
tracks.push_back(tr);
}
}
// Update Kalman Filters state
for(int i=0;i<assignment.size();i++)
{
// If track updated less than one time, than filter state is not correct.
tracks[i]->KF->GetPrediction();
if(assignment[i]!=-1) // If we have assigned detect, then update using its coordinates,
{
tracks[i]->skipped_frames=0;
tracks[i]->prediction=tracks[i]->KF->Update(detections[assignment[i]],1);
}else // if not continue using predictions
{
tracks[i]->prediction=tracks[i]->KF->Update(Point2f(0,0),0);
}
if(tracks[i]->trace.size()>max_trace_length)
{
tracks[i]->trace.erase(tracks[i]->trace.begin(),tracks[i]->trace.end()-max_trace_length);
}
tracks[i]->trace.push_back(tracks[i]->prediction);
tracks[i]->KF->LastResult=tracks[i]->prediction;
}
}
// Process a set of detected rectangles
// Each will either match a previously detected object or
// if not, be added as new object to the list
void TrackedObjectList::processDetect(const vector<Rect> &detectedRects,
const vector<float> &depths,
const vector<ObjectType> &types)
{
vector<Point3f> detectedPositions;
#ifdef VERBOSE_TRACK
if (detectedRects.size() || list_.size())
cout << "---------- Start of process detect --------------" << endl;
print();
if (detectedRects.size() > 0)
cout << detectedRects.size() << " detected objects" << endl;
#endif
for (size_t i = 0; i < detectedRects.size(); i++)
{
detectedPositions.push_back(
screenToWorldCoords(detectedRects[i], depths[i], fovSize_, imageSize_, cameraElevation_));
#ifdef VERBOSE_TRACK
cout << "Detected rect [" << i << "] = " << detectedRects[i] << " positions[" << detectedPositions.size() - 1 << "]:" << detectedPositions[detectedPositions.size()-1] << endl;
#endif
}
// TODO :: Combine overlapping detections into one?
// Maps tracks to the closest new detected object.
// assignment[track] = index of closest detection
vector<int> assignment;
if (list_.size())
{
size_t tracks = list_.size(); // number of tracked objects from prev frames
size_t detections = detectedPositions.size(); // number of detections this frame
//Cost[t][d] is the distance between old tracked location t
//and newly detected object d's position
vector< vector<double> > Cost(tracks,vector<double>(detections));
// Calculate cost for each track->pair combo
// The cost here is just the distance between them
// Also check to see if the types are the same, if they are not then set the cost extremely high so that it's never matched
auto it = list_.cbegin();
for(size_t t = 0; t < tracks; ++t, ++it)
{
// Point3f prediction=tracks[t]->prediction;
// cout << prediction << endl;
for(size_t d = 0; d < detections; d++)
{
const ObjectType it_type = it->getType();
if(types[d] == it_type) {
Point3f diff = it->getPosition() - detectedPositions[d];
Cost[t][d] = sqrtf(diff.x * diff.x + diff.y * diff.y + diff.z * diff.z);
} else {
Cost[t][d] = numeric_limits<double>::max();
}
}
}
// Solving assignment problem (find minimum-cost assignment
// between tracks and previously-predicted positions)
AssignmentProblemSolver APS;
APS.Solve(Cost, assignment, AssignmentProblemSolver::optimal);
#ifdef VERBOSE_TRACK
// assignment[i] holds the index of the detection assigned
// to track i. assignment[i] is -1 if no detection was
// matchedto that particular track
cout << "After APS : "<<endl;
for(size_t i = 0; i < assignment.size(); i++)
cout << i << ":" << assignment[i] << endl;
#endif
// clear assignment from pairs with large distance
for(size_t i = 0; i < assignment.size(); i++)
if ((assignment[i] != -1) && (Cost[i][assignment[i]] > dist_thresh_))
assignment[i] = -1;
}
// Search for unassigned detects and start new tracks for them.
// This will also handle the case where no tracks are present,
// since assignment will be empty in that case - everything gets added
for(size_t i = 0; i < detectedPositions.size(); i++)
{
if (find(assignment.begin(), assignment.end(), i) == assignment.end())
{
#ifdef VERBOSE_TRACK
cout << "New assignment created " << i << endl;
#endif
list_.push_back(TrackedObject(detectCount_++, types[i], detectedRects[i], depths[i], fovSize_, imageSize_, cameraElevation_));
#ifdef VERBOSE_TRACK
cout << "New assignment finished" << endl;
#endif
}
}
auto tr = list_.begin();
auto as = assignment.begin();
while ((tr != list_.end()) && (as != assignment.end()))
{
// If track updated less than one time, than filter state is not correct.
#ifdef VERBOSE_TRACK
cout << "Predict: " << endl;
#endif
Point3f prediction = tr->predictKF();
#ifdef VERBOSE_TRACK
cout << "prediction:" << prediction << endl;
#endif
if(*as != -1) // If we have assigned detect, then update using its coordinates
{
#ifdef VERBOSE_TRACK
cout << "Update match: " << endl;
#endif
tr->setPosition(tr->updateKF(detectedPositions[*as]));
#ifdef VERBOSE_TRACK
cout << tr->getScreenPosition(fovSize_, imageSize_) << endl;
#endif
tr->setDetected();
}
else // if not continue using predictions
{
#ifdef VERBOSE_TRACK
cout << "Update no match: " << endl;
#endif
tr->setPosition(tr->updateKF(prediction));
#ifdef VERBOSE_TRACK
cout << tr->getScreenPosition(fovSize_, imageSize_) << endl;
#endif
tr->clearDetected();
}
++tr;
++as;
}
// Remove tracks which haven't been seen in a while
for (auto it = list_.begin(); it != list_.end(); )
{
if (it->tooManyMissedFrames()) // For now just remove ones for
{ // which detectList is empty
#ifdef VERBOSE_TRACK
cout << "Dropping " << it->getId() << endl;
#endif
it = list_.erase(it);
}
else
{
++it;
}
}
#ifdef VERBOSE_TRACK
print();
if (detectedRects.size() || list_.size())
cout << "---------- End of process detect --------------" << endl;
#endif
}
void MultiObjectTracker::update(const std::vector<cv::Point2f>& massCenters,
const std::vector<cv::Rect>& boundingRects,
std::vector<OT::TrackingOutput>& trackingOutputs) {
trackingOutputs.clear();
// If we haven't found any mass centers, just update all the Kalman filters and return their predictions.
if (massCenters.empty()) {
for (int i = 0; i < this->kalmanTrackers.size(); i++) {
// Indicate that the tracker didn't get an update this frame.
this->kalmanTrackers[i].noUpdateThisFrame();
// Remove the tracker if it is dead.
if (this->kalmanTrackers[i].getNumFramesWithoutUpdate() > this->missedFramesThreshold) {
this->kalmanTrackers.erase(this->kalmanTrackers.begin() + i);
i--;
}
}
// Update the remaining trackers.
for (size_t i = 0; i < this->kalmanTrackers.size(); i++) {
if (this->kalmanTrackers[i].getLifetime() > lifetimeThreshold) {
this->kalmanTrackers[i].predict();
trackingOutputs.push_back(this->kalmanTrackers[i].latestTrackingOutput());
}
}
return;
}
// If there are no Kalman trackers, make one for each detection.
if (this->kalmanTrackers.empty()) {
for (auto massCenter : massCenters) {
this->kalmanTrackers.push_back(OT::KalmanTracker(massCenter,
this->dt,
this->magnitudeOfAccelerationNoise));
}
}
// Create our cost matrix.
size_t numKalmans = this->kalmanTrackers.size();
size_t numCenters = massCenters.size();
std::vector<std::vector<double>> costMatrix(numKalmans, std::vector<double>(numCenters));
std::vector<int> assignment;
// Get the latest prediction for the Kalman filters.
std::vector<cv::Point2f> predictions(this->kalmanTrackers.size());
for (size_t i = 0; i < this->kalmanTrackers.size(); i++) {
predictions[i] = this->kalmanTrackers[i].latestPrediction();
}
// We need to associate each of the mass centers to their corresponding Kalman filter. First,
// let's find the pairwise distances. However, we first divide this distance by the diagonal size
// of the frame to ensure that it is between 0 and 1.
cv::Point framePoint = cv::Point(this->frameSize.width, this->frameSize.height);
double frameDiagonal = std::sqrt(framePoint.dot(framePoint));
for (size_t i = 0; i < predictions.size(); i++) {
for (size_t j = 0; j < massCenters.size(); j++) {
costMatrix[i][j] = cv::norm(predictions[i] - massCenters[j]) / frameDiagonal;
}
}
// Assign Kalman trackers to mass centers with the Hungarian algorithm.
AssignmentProblemSolver solver;
solver.Solve(costMatrix, assignment, AssignmentProblemSolver::optimal);
// Unassign any Kalman trackers whose distance to their assignment is too large.
std::vector<int> kalmansWithoutCenters;
for (size_t i = 0; i < assignment.size(); i++) {
if (assignment[i] != -1) {
if (costMatrix[i][assignment[i]] > this->distanceThreshold) {
assignment[i] = -1;
kalmansWithoutCenters.push_back(i);
}
} else {
this->kalmanTrackers[i].noUpdateThisFrame();
}
}
// If a Kalman tracker is contained in a bounding box and shares its
// bounding box with another tracker, remove its assignment and mark it
// as updated.
for (size_t i = 0; i < assignment.size(); i++) {
for (size_t j = 0; j < boundingRects.size(); j++) {
if (boundingRects[j].contains(this->kalmanTrackers[i].latestPrediction())
&& this->sharesBoundingRect(i, boundingRects[j])) {
this->kalmanTrackers[i].gotUpdate();
break;
}
}
}
// Remove any trackers that haven't been updated in a while.
for (int i = 0; i < this->kalmanTrackers.size(); i++) {
if (this->kalmanTrackers[i].getNumFramesWithoutUpdate() > this->missedFramesThreshold) {
this->kalmanTrackers.erase(this->kalmanTrackers.begin() + i);
assignment.erase(assignment.begin() + i);
i--;
}
}
// Find unassigned mass centers.
std::vector<int> centersWithoutKalman;
std::vector<int>::iterator it;
for (size_t i = 0; i < massCenters.size(); i++) {
it = std::find(assignment.begin(), assignment.end(), i);
if (it == assignment.end()) {
centersWithoutKalman.push_back(i);
}
}
// Create new trackers for the unassigned mass centers.
for (size_t i = 0; i < centersWithoutKalman.size(); i++) {
this->kalmanTrackers.push_back(OT::KalmanTracker(massCenters[centersWithoutKalman[i]]));
}
// Update the Kalman filters.
for (size_t i = 0; i < assignment.size(); i++) {
this->kalmanTrackers[i].predict();
if (assignment[i] != -1) {
this->kalmanTrackers[i].correct(massCenters[assignment[i]]);
this->kalmanTrackers[i].gotUpdate();
}
}
// Remove any suppressed filters.
for (size_t i = 0; i < this->kalmanTrackers.size(); i++) {
if (this->hasSuppressor(i)) {
this->kalmanTrackers.erase(this->kalmanTrackers.begin() + i);
i--;
}
}
// Now update the predictions.
for (size_t i = 0; i < this->kalmanTrackers.size(); i++) {
if (this->kalmanTrackers[i].getLifetime() > this->lifetimeThreshold) {
trackingOutputs.push_back(this->kalmanTrackers[i].latestTrackingOutput());
}
}
}
void CTracker::Update(vector<Point2d>& detections, float starttime)
{
// -----------------------------------
// Если треков еще нет, то начнем для каждой точки по треку
// -----------------------------------
if(tracks.size()==0)
{
// Если еще нет ни одного трека
for(int i=0;i<detections.size();i++)
{
CTrack* tr=new CTrack(detections[i],dt,Accel_noise_mag, _statfileid, _trackfileid, starttime);
tracks.push_back(tr);
}
}
// -----------------------------------
// Здесь треки уже есть в любом случае
// -----------------------------------
int N=tracks.size(); // треки
int M=detections.size(); // детекты
// Матрица расстояний от N-ного трека до M-ного детекта.
vector< vector<double> > Cost(N,vector<double>(M));
vector<int> assignment; // назначения
// -----------------------------------
// Треки уже есть, составим матрицу расстояний
// -----------------------------------
double dist;
for(int i=0;i<tracks.size();i++)
{
// Point2d prediction=tracks[i]->prediction;
// cout << prediction << endl;
for(int j=0;j<detections.size();j++)
{
Point2d diff=(tracks[i]->prediction-detections[j]);
dist=sqrtf(diff.x*diff.x+diff.y*diff.y);
Cost[i][j]=dist;
}
}
// -----------------------------------
// Решаем задачу о назначениях (треки и прогнозы фильтра)
// -----------------------------------
AssignmentProblemSolver APS;
APS.Solve(Cost,assignment,AssignmentProblemSolver::optimal);
// -----------------------------------
// почистим assignment от пар с большим расстоянием
// -----------------------------------
// Не назначенные треки
vector<int> not_assigned_tracks;
for(int i=0;i<assignment.size();i++)
{
if(assignment[i]!=-1)
{
if(Cost[i][assignment[i]]>dist_thres)
{
assignment[i]=-1;
// Отмечаем неназначенные треки, и увеличиваем счетчик пропущеных кадров,
// когда количество пропущенных кадров превысит пороговое значение, трек стирается.
not_assigned_tracks.push_back(i);
}
}
else
{
// Если треку не назначен детект, то увеличиваем счетчик пропущеных кадров.
tracks[i]->skipped_frames++;
}
}
// -----------------------------------
// Если трек долго не получает детектов, удаляем
// -----------------------------------
for(int i=0;i<tracks.size();i++)
{
if(tracks[i]->skipped_frames>maximum_allowed_skipped_frames)
{
delete tracks[i];
tracks.erase(tracks.begin()+i);
assignment.erase(assignment.begin()+i);
i--;
}
}
// -----------------------------------
// Выявляем неназначенные детекты
// -----------------------------------
vector<int> not_assigned_detections;
vector<int>::iterator it;
for(int i=0;i<detections.size();i++)
{
it=find(assignment.begin(), assignment.end(), i);
if(it==assignment.end())
{
not_assigned_detections.push_back(i);
}
}
// -----------------------------------
// и начинаем для них новые треки
// -----------------------------------
if(not_assigned_detections.size()!=0)
{
for(int i=0;i<not_assigned_detections.size();i++)
{
CTrack* tr=new CTrack(detections[not_assigned_detections[i]],dt,Accel_noise_mag, _statfileid, _trackfileid, starttime);
tracks.push_back(tr);
}
}
// Апдейтим состояние фильтров
for(int i=0;i<assignment.size();i++)
{
// Если трек апдейтился меньше одного раза, то состояние фильтра некорректно.
tracks[i]->KF->GetPrediction();
if(assignment[i]!=-1) // Если назначение есть то апдейтим по нему
{
tracks[i]->skipped_frames=0;
tracks[i]->prediction=tracks[i]->KF->Update(detections[assignment[i]],1);
}
else // Если нет, то продолжаем прогнозировать
{
tracks[i]->prediction=tracks[i]->KF->Update(Point2f(0,0),0);
}
/*
if(tracks[i]->trace.size()>max_trace_length)
{
tracks[i]->trace.erase(tracks[i]->trace.begin(),tracks[i]->trace.end()-max_trace_length);
}
*/
tracks[i]->trace.push_back(tracks[i]->prediction);
// store real path or the prediction if missed position
if(assignment[i]!=-1)
{
tracks[i]->realtrace.push_back(detections[assignment[i]]);
}
else
{
tracks[i]->realtrace.push_back(tracks[i]->prediction);
}
tracks[i]->KF->LastResult=tracks[i]->prediction;
}
}
void CTracker::Update(vector<Point2f>& detections)
{
// If there is no tracks yet, then every point begins its own track.
if (tracks.size() == 0)
{
// If no tracks yet
for (int i = 0; i < detections.size(); ++i)
{
CTrack* tr = new CTrack(detections[i], dt, Accel_noise_mag);
tracks.push_back(tr);
}
}
int N = tracks.size();
int M = detections.size();
vector<vector<float>> Cost(N, vector<float>(M));
vector<int> assignment;
float dist;
for (int i = 0; i < tracks.size(); ++i)
{
// Point2f prediction=tracks[i]->prediction;
// console_log(to_string(prediction.x) + to_string(prediction.y));
for (int j = 0; j < detections.size(); ++j)
{
Point2f diff = (tracks[i]->prediction - detections[j]);
dist = sqrtf(diff.x * diff.x + diff.y * diff.y);
Cost[i][j] = dist;
}
}
// Solving assignment problem (tracks and predictions of Kalman filter)
AssignmentProblemSolver APS;
APS.Solve(Cost, assignment, AssignmentProblemSolver::optimal);
// clean assignment from pairs with large distance
// Not assigned tracks
vector<int> not_assigned_tracks;
for (int i = 0; i < assignment.size(); ++i)
{
if (assignment[i] != -1)
{
if (Cost[i][assignment[i]] > dist_thres)
{
assignment[i] = -1;
// Mark unassigned tracks, and increment skipped frames counter,
// when skipped frames counter will be larger than threshold, track will be deleted.
not_assigned_tracks.push_back(i);
}
}
else
{
// If track have no assigned detect, then increment skipped frames counter.
tracks[i]->skipped_frames++;
}
}
// If track didn't get detects long time, remove it.
for (int i = 0; i < tracks.size(); ++i)
{
if (tracks[i]->skipped_frames > maximum_allowed_skipped_frames)
{
delete tracks[i];
tracks.erase(tracks.begin() + i);
assignment.erase(assignment.begin() + i);
--i;
}
}
// Search for unassigned detects
vector<int> not_assigned_detections;
vector<int>::iterator it;
for (int i = 0; i < detections.size(); ++i)
{
it = find(assignment.begin(), assignment.end(), i);
if (it == assignment.end())
{
not_assigned_detections.push_back(i);
}
}
// and start new tracks for them.
if (not_assigned_detections.size() != 0)
{
for (int i = 0; i < not_assigned_detections.size(); ++i)
{
CTrack* tr = new CTrack(detections[not_assigned_detections[i]], dt,Accel_noise_mag);
tracks.push_back(tr);
}
}
// Update Kalman Filters state
for (int i = 0; i < assignment.size(); ++i)
{
// If track updated less than one time, than filter state is not correct.
tracks[i]->KF->GetPrediction();
if (assignment[i] != -1) // If we have assigned detect, then update using its coordinates,
{
tracks[i]->skipped_frames = 0;
tracks[i]->prediction = tracks[i]->KF->Update(detections[assignment[i]], 1);
tracks[i]->raw = detections[assignment[i]];
}
else // if not continue using predictions
{
tracks[i]->prediction = tracks[i]->KF->Update(Point2f(0, 0), 0);
tracks[i]->raw = Point2f(0, 0);
}
if(tracks[i]->trace.size() > max_trace_length)
{
tracks[i]->trace.erase(tracks[i]->trace.begin(), tracks[i]->trace.end() - max_trace_length);
}
tracks[i]->trace.push_back(tracks[i]->prediction);
tracks[i]->KF->LastResult = tracks[i]->prediction;
}
}
// ---------------------------------------------------------------------------
//param rects input/output bounding boxes
// ---------------------------------------------------------------------------
void CTracker::Update(
const std::vector<Point_t>& detections,
const std::vector<cv::Rect>& rects,
DistType distType
)
{
assert(detections.size() == rects.size());
SWRI_PROFILE("Tracking");
// -----------------------------------
// If there is no tracks yet, then every cv::Point begins its own track.
// -----------------------------------
if (tracks.size() == 0)
{
// If no tracks yet
for (size_t i = 0; i < detections.size(); ++i)
{
tracks.push_back(std::make_shared<CTrack>(detections[i], rects[i], dt, Accel_noise_mag, NextTrackID++));
}
}
size_t N = tracks.size(); // треки
size_t M = detections.size(); // детекты
assignments_t assignment; // назначения
if (!tracks.empty())
{
// Матрица расстояний от N-ного трека до M-ного детекта.
distMatrix_t Cost(N * M);
// -----------------------------------
// Треки уже есть, составим матрицу расстояний
// -----------------------------------
switch (distType)
{
case CentersDist:
for (size_t i = 0; i < tracks.size(); i++)
{
for (size_t j = 0; j < detections.size(); j++)
{
Cost[i + j * N] = tracks[i]->CalcDist(detections[j]);
}
}
break;
case RectsDist:
for (size_t i = 0; i < tracks.size(); i++)
{
for (size_t j = 0; j < detections.size(); j++)
{
Cost[i + j * N] = tracks[i]->CalcDist(rects[j]);
}
}
break;
}
// -----------------------------------
// Solving assignment problem (tracks and predictions of Kalman filter)
// -----------------------------------
AssignmentProblemSolver APS;
APS.Solve(Cost, N, M, assignment, AssignmentProblemSolver::optimal);
// -----------------------------------
// clean assignment from pairs with large distance
// -----------------------------------
for (size_t i = 0; i < assignment.size(); i++)
{
if (assignment[i] != -1)
{
//std::cout << "Kalman cost: " << Cost[i + assignment[i] * N] << std::endl;
if (Cost[i + assignment[i] * N] > dist_thres)
{
assignment[i] = -1;
tracks[i]->skipped_frames++;
}
}
else
{
// If track have no assigned detect, then increment skipped frames counter.
tracks[i]->skipped_frames++;
}
}
// -----------------------------------
// If track didn't get detects long time, remove it.
// -----------------------------------
for (int i = 0; i < static_cast<int>(tracks.size()); i++)
{
if (tracks[i]->skipped_frames > maximum_allowed_skipped_frames)
{
tracks.erase(tracks.begin() + i);
assignment.erase(assignment.begin() + i);
i--;
}
}
}
// -----------------------------------
// Search for unassigned detects and start new tracks for them.
// -----------------------------------
for (size_t i = 0; i < detections.size(); ++i)
{
if (find(assignment.begin(), assignment.end(), i) == assignment.end())
{
tracks.push_back(std::make_shared<CTrack>(detections[i], rects[i], dt, Accel_noise_mag, NextTrackID++));
}
}
// Update Kalman Filters state
for (size_t i = 0; i<assignment.size(); i++)
{
// If track updated less than one time, than filter state is not correct.
if (assignment[i] != -1) // If we have assigned detect, then update using its coordinates,
{
tracks[i]->skipped_frames = 0;
tracks[i]->seen_frames++;
tracks[i]->Update(detections[assignment[i]], rects[assignment[i]], true, max_trace_length);
}
else // if not continue using predictions
{
tracks[i]->Update(Point_t(), cv::Rect(), false, max_trace_length);
}
}
}
