Ctracker.cpp
Go to the documentation of this file.
00001 // Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3
00002 // Refer to README.md in this directory.
00003 
00004 #include <costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h>
00005 
00006 // ---------------------------------------------------------------------------
00007 // Tracker. Manage tracks. Create, remove, update.
00008 // ---------------------------------------------------------------------------
00009 CTracker::CTracker(const Params &parameters)
00010     : params(parameters),
00011       NextTrackID(0)
00012 {
00013 }
00014 // ---------------------------------------------------------------------------
00015 //
00016 // ---------------------------------------------------------------------------
00017 void CTracker::Update(const std::vector<Point_t>& detectedCentroid, const std::vector< std::vector<cv::Point> >& contours)
00018 {
00019   // Each contour has a centroid
00020   assert(detectedCentroid.size() == contours.size());
00021 
00022   // -----------------------------------
00023   // If there is no tracks yet, then every cv::Point begins its own track.
00024   // -----------------------------------
00025   if (tracks.size() == 0)
00026   {
00027     // If no tracks yet
00028     for (size_t i = 0; i < detectedCentroid.size(); ++i)
00029     {
00030       tracks.push_back(
00031           std::unique_ptr<CTrack>(new CTrack(detectedCentroid[i], contours[i], params.dt, NextTrackID++)));
00032     }
00033   }
00034 
00035   size_t N = tracks.size();
00036   size_t M = detectedCentroid.size();
00037 
00038   assignments_t assignment;
00039 
00040   if (!tracks.empty())
00041   {
00042     // Distance matrix of N-th Track to the M-th detectedCentroid
00043     distMatrix_t Cost(N * M);
00044 
00045     // calculate distance between the blobs centroids
00046     for (size_t i = 0; i < tracks.size(); i++)
00047     {
00048       for (size_t j = 0; j < detectedCentroid.size(); j++)
00049       {
00050         Cost[i + j * N] = tracks[i]->CalcDist(detectedCentroid[j]);
00051       }
00052     }
00053 
00054     // -----------------------------------
00055     // Solving assignment problem (tracks and predictions of Kalman filter)
00056     // -----------------------------------
00057     AssignmentProblemSolver APS;
00058     APS.Solve(Cost, N, M, assignment, AssignmentProblemSolver::optimal);
00059 
00060     // -----------------------------------
00061     // clean assignment from pairs with large distance
00062     // -----------------------------------
00063     for (size_t i = 0; i < assignment.size(); i++)
00064     {
00065       if (assignment[i] != -1)
00066       {
00067         if (Cost[i + assignment[i] * N] > params.dist_thresh)
00068         {
00069           assignment[i] = -1;
00070           tracks[i]->skipped_frames = 1;
00071         }
00072       }
00073       else
00074       {
00075         // If track have no assigned detect, then increment skipped frames counter.
00076         tracks[i]->skipped_frames++;
00077       }
00078     }
00079 
00080     // -----------------------------------
00081     // If track didn't get detects long time, remove it.
00082     // -----------------------------------
00083     for (int i = 0; i < static_cast<int>(tracks.size()); i++)
00084     {
00085       if (tracks[i]->skipped_frames > params.max_allowed_skipped_frames)
00086       {
00087         tracks.erase(tracks.begin() + i);
00088         assignment.erase(assignment.begin() + i);
00089         i--;
00090       }
00091     }
00092   }
00093 
00094   // -----------------------------------
00095   // Search for unassigned detects and start new tracks for them.
00096   // -----------------------------------
00097   for (size_t i = 0; i < detectedCentroid.size(); ++i)
00098   {
00099     if (find(assignment.begin(), assignment.end(), i) == assignment.end())
00100     {
00101       tracks.push_back(std::unique_ptr<CTrack>(new CTrack(detectedCentroid[i], contours[i], params.dt, NextTrackID++)));
00102     }
00103   }
00104 
00105   // Update Kalman Filters state
00106 
00107   for (size_t i = 0; i < assignment.size(); i++)
00108   {
00109     // If track updated less than one time, than filter state is not correct.
00110 
00111     if (assignment[i] != -1) // If we have assigned detect, then update using its coordinates,
00112     {
00113       tracks[i]->skipped_frames = 0;
00114       tracks[i]->Update(detectedCentroid[assignment[i]], contours[assignment[i]], true, params.max_trace_length);
00115     }
00116     else // if not continue using predictions
00117     {
00118       tracks[i]->Update(Point_t(), std::vector<cv::Point>(), false, params.max_trace_length);
00119     }
00120   }
00121 }
00122 
00123 void CTracker::updateParameters(const Params &parameters)
00124 {
00125   params = parameters;
00126 }
00127 // ---------------------------------------------------------------------------
00128 //
00129 // ---------------------------------------------------------------------------
00130 CTracker::~CTracker(void) {}


costmap_converter
Author(s): Christoph Rösmann , Franz Albers , Otniel Rinaldo
autogenerated on Wed Sep 20 2017 03:00:37