Ctracker.h
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 #pragma once
00005 #include "Kalman.h"
00006 #include "HungarianAlg.h"
00007 #include "defines.h"
00008 #include <iostream>
00009 #include <vector>
00010 #include <memory>
00011 #include <array>
00012 
00013 // --------------------------------------------------------------------------
00014 class CTrack
00015 {
00016 public:
00017   CTrack(const Point_t& p, const std::vector<cv::Point>& contour, track_t dt, size_t trackID)
00018       : track_id(trackID),
00019         skipped_frames(0),
00020         prediction(p),
00021         lastContour(contour),
00022         KF(p, dt)
00023   {
00024   }
00025 
00026   track_t CalcDist(const Point_t& p)
00027   {
00028     Point_t diff = prediction - p;
00029     return std::sqrt(diff.x * diff.x + diff.y * diff.y + diff.z * diff.z);
00030   }
00031 
00032   void Update(const Point_t& p, const std::vector<cv::Point>& contour, bool dataCorrect, size_t max_trace_length)
00033   {
00034     KF.Prediction();
00035     prediction = KF.Update(p, dataCorrect);
00036 
00037     if (dataCorrect)
00038     {
00039       lastContour = contour;
00040     }
00041 
00042     if (trace.size() > max_trace_length)
00043     {
00044       trace.erase(trace.begin(), trace.end() - max_trace_length);
00045     }
00046 
00047     trace.push_back(prediction);
00048   }
00049 
00050   // Returns contour in [px], not in [m]!
00051   std::vector<cv::Point> getLastContour() const
00052   {
00053     return lastContour;
00054   }
00055 
00056   // Returns velocity in [px/s], not in [m/s]!
00057   Point_t getEstimatedVelocity() const
00058   {
00059     return KF.LastVelocity;
00060   }
00061 
00062   std::vector<Point_t> trace;
00063   size_t track_id;
00064   size_t skipped_frames;
00065 
00066 private:
00067   Point_t prediction;
00068   std::vector<cv::Point> lastContour; // Contour liegt immer auf ganzen Pixeln -> Integer Punkt
00069   TKalmanFilter KF;
00070 };
00071 
00072 // --------------------------------------------------------------------------
00073 class CTracker
00074 {
00075 public:
00076   struct Params{
00077     track_t dt; // time for one step of the filter
00078     track_t dist_thresh;// distance threshold. Pairs of points with higher distance are not considered in the assignment problem
00079     int max_allowed_skipped_frames; // Maximum number of frames the track is maintained without seeing the object in the measurement data
00080     int max_trace_length; // Maximum trace length
00081   };
00082 
00083   CTracker(const Params& parameters);
00084   ~CTracker(void);
00085 
00086   std::vector<std::unique_ptr<CTrack>> tracks;
00087   void Update(const std::vector<Point_t>& detectedCentroid, const std::vector<std::vector<cv::Point> > &contour);
00088 
00089   void updateParameters(const Params &parameters);
00090 
00091 private:
00092   // Aggregated parameters for the tracker object
00093   Params params;
00094   // ID for the upcoming CTrack object
00095   size_t NextTrackID;
00096 };


costmap_converter
Author(s): Christoph Rösmann , Franz Albers , Otniel Rinaldo
autogenerated on Sat Jun 8 2019 20:53:15