61 lines
2.0 KiB
C
61 lines
2.0 KiB
C
|
#ifndef UCMCKALMAN_H
|
||
|
|
#define UCMCKALMAN_H
|
||
|
|
#pragma once
|
||
|
|
#include <Eigen/Eigen>
|
||
|
|
#include <Eigen/Dense>
|
||
|
|
#include <unordered_map>
|
||
|
|
namespace UCMCKalman {
|
||
|
|
enum struct TrackStatus { Tentative, Confirmed, Coasted, Deleted };
|
||
|
|
|
||
|
|
void lapjv(const Eigen::MatrixXd& costMatrix, double costLimit,
|
||
|
|
std::vector<int>& x, std::vector<int>& y);
|
||
|
|
void linearAssignment(const Eigen::MatrixXd& costMatrix, double thresh,
|
||
|
|
std::vector<std::vector<int>>& matches,
|
||
|
|
std::vector<int>& unmatched_a,
|
||
|
|
std::vector<int>& unmatched_b);
|
||
|
|
|
||
|
|
class Filter {
|
||
|
|
public:
|
||
|
|
Filter(int dim_x, int dim_z, int dim_u = 0);
|
||
|
|
void predict();
|
||
|
|
void update(const Eigen::VectorXd& z,const Eigen::MatrixXd& R);
|
||
|
|
[[nodiscard]] Eigen::VectorXd getState() const;
|
||
|
|
[[nodiscard]] Eigen::MatrixXd getCovariance() const;
|
||
|
|
|
||
|
|
// private:
|
||
|
|
int dim_x, dim_z, dim_u;
|
||
|
|
Eigen::VectorXd x, x_prior, x_post;
|
||
|
|
Eigen::MatrixXd F, P, Q, H, R, M, z, K, y, S, SI, _I, P_prior, P_post;
|
||
|
|
double _alpha_sq;
|
||
|
|
};
|
||
|
|
class Tracker {
|
||
|
|
public:
|
||
|
|
Tracker(Eigen::Vector2d y, Eigen::Matrix2d R, double wx, double wy, double vmax,
|
||
|
|
double w, double h, double dt = 1.0 / 30.0, int tracker_count = 0);
|
||
|
|
void update(const Eigen::Vector2d& y, Eigen::Matrix2d& R);
|
||
|
|
[[nodiscard]] Eigen::Vector2d predict();
|
||
|
|
[[nodiscard]] Eigen::Vector4d getState() const;
|
||
|
|
[[nodiscard]] double distance(const Eigen::Vector2d& y, Eigen::Matrix2d& R, bool show) const;
|
||
|
|
|
||
|
|
int id;
|
||
|
|
int age;
|
||
|
|
int death_count;
|
||
|
|
int birth_count;
|
||
|
|
int detidx;
|
||
|
|
double w;
|
||
|
|
double h;
|
||
|
|
TrackStatus status;
|
||
|
|
int class_id_;
|
||
|
|
void voteClassId(int new_class_id, float score);
|
||
|
|
|
||
|
|
private:
|
||
|
|
Filter kf;
|
||
|
|
std::unordered_map<int, float> class_id_scores_;
|
||
|
|
int detection_count_;
|
||
|
|
bool class_id_locked_;
|
||
|
|
static const int CLASS_ID_LOCK_FRAMES = 10;
|
||
|
|
};
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif
|