Ctracker.cpp
Go to the documentation of this file.
1 // Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
2 // Refer to README.md in this directory.
3 
5 
6 // ---------------------------------------------------------------------------
7 // Tracker. Manage tracks. Create, remove, update.
8 // ---------------------------------------------------------------------------
9 CTracker::CTracker(const Params &parameters)
10  : params(parameters),
11  NextTrackID(0)
12 {
13 }
14 // ---------------------------------------------------------------------------
15 //
16 // ---------------------------------------------------------------------------
17 void CTracker::Update(const std::vector<Point_t>& detectedCentroid, const std::vector< std::vector<cv::Point> >& contours)
18 {
19  // Each contour has a centroid
20  assert(detectedCentroid.size() == contours.size());
21 
22  // -----------------------------------
23  // If there is no tracks yet, then every cv::Point begins its own track.
24  // -----------------------------------
25  if (tracks.size() == 0)
26  {
27  // If no tracks yet
28  for (size_t i = 0; i < detectedCentroid.size(); ++i)
29  {
30  tracks.push_back(
31  std::unique_ptr<CTrack>(new CTrack(detectedCentroid[i], contours[i], params.dt, NextTrackID++)));
32  }
33  }
34 
35  size_t N = tracks.size();
36  size_t M = detectedCentroid.size();
37 
38  assignments_t assignment;
39 
40  if (!tracks.empty())
41  {
42  // Distance matrix of N-th Track to the M-th detectedCentroid
43  distMatrix_t Cost(N * M);
44 
45  // calculate distance between the blobs centroids
46  for (size_t i = 0; i < tracks.size(); i++)
47  {
48  for (size_t j = 0; j < detectedCentroid.size(); j++)
49  {
50  Cost[i + j * N] = tracks[i]->CalcDist(detectedCentroid[j]);
51  }
52  }
53 
54  // -----------------------------------
55  // Solving assignment problem (tracks and predictions of Kalman filter)
56  // -----------------------------------
58  APS.Solve(Cost, N, M, assignment, AssignmentProblemSolver::optimal);
59 
60  // -----------------------------------
61  // clean assignment from pairs with large distance
62  // -----------------------------------
63  for (size_t i = 0; i < assignment.size(); i++)
64  {
65  if (assignment[i] != -1)
66  {
67  if (Cost[i + assignment[i] * N] > params.dist_thresh)
68  {
69  assignment[i] = -1;
70  tracks[i]->skipped_frames = 1;
71  }
72  }
73  else
74  {
75  // If track have no assigned detect, then increment skipped frames counter.
76  tracks[i]->skipped_frames++;
77  }
78  }
79 
80  // -----------------------------------
81  // If track didn't get detects long time, remove it.
82  // -----------------------------------
83  for (int i = 0; i < static_cast<int>(tracks.size()); i++)
84  {
85  if (tracks[i]->skipped_frames > params.max_allowed_skipped_frames)
86  {
87  tracks.erase(tracks.begin() + i);
88  assignment.erase(assignment.begin() + i);
89  i--;
90  }
91  }
92  }
93 
94  // -----------------------------------
95  // Search for unassigned detects and start new tracks for them.
96  // -----------------------------------
97  for (size_t i = 0; i < detectedCentroid.size(); ++i)
98  {
99  if (find(assignment.begin(), assignment.end(), i) == assignment.end())
100  {
101  tracks.push_back(std::unique_ptr<CTrack>(new CTrack(detectedCentroid[i], contours[i], params.dt, NextTrackID++)));
102  }
103  }
104 
105  // Update Kalman Filters state
106 
107  for (size_t i = 0; i < assignment.size(); i++)
108  {
109  // If track updated less than one time, than filter state is not correct.
110 
111  if (assignment[i] != -1) // If we have assigned detect, then update using its coordinates,
112  {
113  tracks[i]->skipped_frames = 0;
114  tracks[i]->Update(detectedCentroid[assignment[i]], contours[assignment[i]], true, params.max_trace_length);
115  }
116  else // if not continue using predictions
117  {
118  tracks[i]->Update(Point_t(), std::vector<cv::Point>(), false, params.max_trace_length);
119  }
120  }
121 }
122 
123 void CTracker::updateParameters(const Params &parameters)
124 {
125  params = parameters;
126 }
127 // ---------------------------------------------------------------------------
128 //
129 // ---------------------------------------------------------------------------
std::vector< std::unique_ptr< CTrack > > tracks
Definition: Ctracker.h:86
int max_trace_length
Definition: Ctracker.h:80
CTracker(const Params &parameters)
Definition: Ctracker.cpp:9
int max_allowed_skipped_frames
Definition: Ctracker.h:79
cv::Point3_< track_t > Point_t
Definition: defines.h:8
void updateParameters(const Params &parameters)
Definition: Ctracker.cpp:123
~CTracker(void)
Definition: Ctracker.cpp:130
track_t dist_thresh
Definition: Ctracker.h:78
size_t NextTrackID
Definition: Ctracker.h:95
std::vector< track_t > distMatrix_t
Definition: HungarianAlg.h:13
Params params
Definition: Ctracker.h:93
track_t dt
Definition: Ctracker.h:77
std::vector< int > assignments_t
Definition: HungarianAlg.h:12
bool find(const std::vector< unsigned int > &l, unsigned int n)
track_t Solve(const distMatrix_t &distMatrixIn, size_t nOfRows, size_t nOfColumns, assignments_t &assignment, TMethod Method=optimal)
void Update(const std::vector< Point_t > &detectedCentroid, const std::vector< std::vector< cv::Point > > &contour)
Definition: Ctracker.cpp:17


costmap_converter
Author(s): Christoph Rösmann , Franz Albers , Otniel Rinaldo
autogenerated on Fri Jun 7 2019 21:48:43