127 lines
3.7 KiB
C++
127 lines
3.7 KiB
C++
|
|
#ifndef UCMCTRACK_H
|
|
#define UCMCTRACK_H
|
|
#define UCMCTRACK_API __declspec(dllexport)
|
|
#pragma once
|
|
#include <opencv2/opencv.hpp>
|
|
#include <Eigen/Eigen>
|
|
#include <Eigen/Dense>
|
|
#include <cmath>
|
|
#include <iostream>
|
|
#include <chrono>
|
|
#include "UCMClapjv.h"
|
|
#include "UCMCKalman.h"
|
|
namespace UCMC {
|
|
const int color_list[][3] = {
|
|
{255, 56, 56},
|
|
{255, 157, 151},
|
|
{255, 112, 31},
|
|
{255, 178, 29},
|
|
{207, 210, 49},
|
|
{72, 249, 10},
|
|
{146, 204, 23},
|
|
{61, 219, 134},
|
|
{26, 147, 52},
|
|
{0, 212, 187},
|
|
{44, 153, 168},
|
|
{0, 194, 255},
|
|
{52, 69, 147},
|
|
{100, 115, 255},
|
|
{0, 24, 236},
|
|
{132, 56, 255},
|
|
{82, 0, 133},
|
|
{203, 56, 255},
|
|
{255, 149, 200},
|
|
{255, 55, 198}
|
|
};
|
|
struct Params {
|
|
double a1 = 100.;
|
|
double a2 = 100.;
|
|
double wx = 5.;
|
|
double wy = 5.;
|
|
double vmax = 10.;
|
|
double max_age = 10.;
|
|
double high_score = 0.5;
|
|
double conf_threshold = 0.01;
|
|
double dt = 0.033;
|
|
std::string dataset = "MOT";
|
|
std::vector<double> Ki;
|
|
std::vector<double> Ko;
|
|
};
|
|
struct Object
|
|
{
|
|
cv::Rect_<float> rect;
|
|
int label;
|
|
float prob;
|
|
std::string object_id;
|
|
};
|
|
struct Obj {
|
|
int id = 0;
|
|
int track_idx = 0;
|
|
Object obj;
|
|
Eigen::Matrix<double, 2, 1> y;
|
|
Eigen::Matrix2d R;
|
|
};
|
|
|
|
class UCMCTRACK_API Mapper {
|
|
Eigen::Matrix3d A, InvA;
|
|
Eigen::MatrixXd KiKo;
|
|
public:
|
|
bool debug = false;
|
|
Mapper() {};
|
|
Mapper(std::vector<double>& _Ki, std::vector<double>& _Ko);
|
|
[[nodiscard]] std::vector<double> uvError(cv::Rect box);
|
|
void uv2xy(Eigen::MatrixXd uv, Eigen::MatrixXd sigma_uv,
|
|
Eigen::Matrix<double, 2, 1>& xy, Eigen::Matrix2d& sigma_xy);
|
|
void map_to(cv::Rect box, Eigen::Matrix<double, 2, 1>& y, Eigen::Matrix2d& R);
|
|
};
|
|
class UCMCTRACK_API Tracker
|
|
{
|
|
private:
|
|
Params params;
|
|
std::vector<UCMCKalman::Tracker> trackers;
|
|
int frame_idx = 0;
|
|
int tracker_count = 0;
|
|
std::vector<int> confirmed_idx, coasted_idx, tentative_idx, detidx_remain;
|
|
|
|
// Frame rate auto-estimation
|
|
bool auto_frame_rate_ = true;
|
|
float estimated_fps_ = 30.0f;
|
|
float time_scale_factor_ = 1.0f;
|
|
size_t fps_sample_count_ = 0;
|
|
std::chrono::steady_clock::time_point last_update_time_;
|
|
bool has_last_update_time_ = false;
|
|
|
|
void estimateFrameRate();
|
|
|
|
public:
|
|
Mapper mapper;
|
|
bool debug = false;
|
|
Tracker(Params& params);
|
|
Tracker() {};
|
|
void update_parameters(double a1, double a2,
|
|
double wx, double wy,
|
|
double vmax, double max_age,
|
|
double high_score, double conf_threshold,
|
|
double dt, const std::vector<double>& Ki,
|
|
const std::vector<double>& Ko,
|
|
bool autoFrameRate = true);
|
|
[[nodiscard]] float getEstimatedFps() const;
|
|
[[nodiscard]] std::vector<Obj> update(std::vector<Object>& det_results);
|
|
void update(std::vector<Obj>& dets, int frame_id);
|
|
void data_association(std::vector<Obj>& dets, int frame_id);
|
|
void associate_tentative(std::vector<Obj>& dets);
|
|
void initial_tentative(std::vector<Obj>& dets);
|
|
void delete_old_trackers();
|
|
void update_status(std::vector<Obj>& dets);
|
|
~Tracker();
|
|
};
|
|
|
|
static cv::Scalar get_color(int index);
|
|
static cv::Mat draw_boxes(cv::Mat image,
|
|
std::vector<UCMC::Obj>& objects,
|
|
std::vector<std::string>& names,
|
|
int draw_size = 20,
|
|
bool draw_label = true);
|
|
}
|
|
#endif |