ANSCENTER/ANSCORE
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial setup for CLion

Browse Source
This commit is contained in:
Tuan Nghia Nguyen
2026-03-28 16:54:11 +11:00
parent 239cc02591
commit 7b4134133c
1136 changed files with 811916 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
ANSMOT/OCSort/include/Association.h Normal file
View File

@@ -0,0 +1,18 @@
#ifndef ASSOCIATION_H
#define ASSOCIATION_H
#include <Eigen/Dense>
#include <algorithm>
#include <vector>
#include "OCSortlapjv.h"
#define m_pi 3.1415926
namespace ANSOCSort {
std::tuple<Eigen::MatrixXf, Eigen::MatrixXf> speed_direction_batch(const Eigen::MatrixXf& dets,
const Eigen::MatrixXf& tracks);
Eigen::MatrixXf iou_batch(const Eigen::MatrixXf& bboxes1, const Eigen::MatrixXf& bboxes2);
Eigen::MatrixXf giou_batch(const Eigen::MatrixXf& bboxes1, const Eigen::MatrixXf& bboxes2);
std::tuple<std::vector<Eigen::Matrix<int, 1, 2>>, std::vector<int>, std::vector<int>> associate(Eigen::MatrixXf detections, Eigen::MatrixXf trackers, float iou_threshold, Eigen::MatrixXf velocities, Eigen::MatrixXf previous_obs_, float vdc_weight);
}
#endif

54
ANSMOT/OCSort/include/KalmanBoxTracker.h Normal file
View File

@@ -0,0 +1,54 @@
#ifndef KALMANBOXTRACKER_H
#define KALMANBOXTRACKER_H
#include "OCSortKalmanFilter.h"
#include "OCSortUtilities.h"
#include "iostream"
/*
This class represents the internal state of individual
tracked objects observed as bbox.
*/
namespace ANSOCSort {
class KalmanBoxTracker {
public:
/*method*/
KalmanBoxTracker() : kf(nullptr) {};
KalmanBoxTracker(Eigen::VectorXf bbox_,int delta_t_ = 3);
~KalmanBoxTracker();
// Deep copy (raw pointer ownership)
KalmanBoxTracker(const KalmanBoxTracker& other);
KalmanBoxTracker& operator=(const KalmanBoxTracker& other);
// Move semantics
KalmanBoxTracker(KalmanBoxTracker&& other) noexcept;
KalmanBoxTracker& operator=(KalmanBoxTracker&& other) noexcept;
void update(Eigen::Matrix<float, 9, 1>* bbox_);
Eigen::RowVectorXf predict();
Eigen::VectorXf get_state();
public:
/*variable*/
static int count;
Eigen::VectorXf bbox;// [9,1]
KalmanFilterNew* kf;
int time_since_update;
int id;
std::vector<Eigen::VectorXf> history;
int hits;
int hit_streak;
int age = 0;
Eigen::RowVectorXf last_observation = Eigen::RowVectorXf::Zero(9);
std::unordered_map<int, Eigen::VectorXf> observations;
std::vector<Eigen::VectorXf> history_observations;
Eigen::RowVectorXf velocity = Eigen::RowVectorXf::Zero(2);// [2,1]
int delta_t;
double GetScore() { return bbox[4]; }
int GetClassId() { return (int)bbox[5]; }
int GetDGId() { return (int)bbox[6]; }
int GetCamId() { return (int)bbox[7]; }
int GetModelId() { return (int)bbox[8]; }
};
}
#endif

47
ANSMOT/OCSort/include/OCSort.h Normal file
View File

@@ -0,0 +1,47 @@
#ifndef OCSORT_H
#define OCSORT_H
#define OCSORT_API __declspec(dllexport)
#include "KalmanBoxTracker.h"
#include "Association.h"
#include "OCSortlapjv.h"
#include <functional>
#include <unordered_map>
#include <chrono>
#include "OCSortObject.h"
namespace ANSOCSort {
class OCSORT_API OCSort {
public:
OCSort();
void update_parameters(float det_thresh_, int max_age_ = 30, int min_hits_ = 3, float iou_threshold_ = 0.3,
int delta_t_ = 3, std::string asso_func_ = "iou", float inertia_ = 0.2, bool use_byte_ = false,
bool autoFrameRate = true);
float getEstimatedFps() const;
std::vector<Eigen::RowVectorXf> update(Eigen::MatrixXf dets);
public:
float det_thresh;
int max_age;
int min_hits;
float iou_threshold;
int delta_t;
std::function<Eigen::MatrixXf(const Eigen::MatrixXf&, const Eigen::MatrixXf&)> asso_func;
float inertia;
bool use_byte;
std::vector<KalmanBoxTracker> trackers;
int frame_count;
private:
// Frame rate auto-estimation
bool auto_frame_rate_;
float estimated_fps_;
float time_scale_factor_;
size_t fps_sample_count_;
std::chrono::steady_clock::time_point last_update_time_;
bool has_last_update_time_;
void estimateFrameRate();
};
}
#endif

99
ANSMOT/OCSort/include/OCSortKalmanFilter.h Normal file
View File

@@ -0,0 +1,99 @@
#ifndef OCSORTKALMANFILTER_H
#define OCSORTKALMANFILTER_H
#include <Eigen/Dense>
#include <any>
#include <map>
#include <vector>
namespace ANSOCSort {
class KalmanFilterNew {
public:
KalmanFilterNew();
KalmanFilterNew(int dim_x_, int dim_z_);
void predict();
void update(Eigen::VectorXf* z_);
void freeze();
void unfreeze();
KalmanFilterNew& operator=(const KalmanFilterNew&) = default;
public:
int dim_z = 4;
int dim_x = 7;
int dim_u = 0;
/// state: This is the Kalman state variable [7,1].
Eigen::VectorXf x;
// P: Covariance matrix. Initially declared as an identity matrix. Data type is float. [7,7].
Eigen::MatrixXf P;
// Q: Process noise covariance matrix. [7,7].
Eigen::MatrixXf Q;
// B: Control matrix. Not used in target tracking. [n,n].
Eigen::MatrixXf B;
// F: Prediction matrix / state transition matrix. [7,7].
Eigen::Matrix<float, 7, 7> F;
// H: Observation model / matrix. [4,7].
Eigen::Matrix<float, 4, 7> H;
// R: Observation noise covariance matrix. [4,4].
Eigen::Matrix<float, 4, 4> R;
// _alpha_sq: Fading memory control, controlling the update weight. Float.
float _alpha_sq = 1.0;
// M: Measurement matrix, converting state vector x to measurement vector z. [7,4]. It has the opposite effect of matrix H.
Eigen::MatrixXf M;
// z: Measurement vector. [4,1].
Eigen::VectorXf z;
/* The following variables are intermediate variables used in calculations */
// K: Kalman gain. [7,4].
Eigen::MatrixXf K;
// y: Measurement residual. [4,1].
Eigen::MatrixXf y;
// S: Measurement residual covariance.
Eigen::MatrixXf S;
// SI: Transpose of measurement residual covariance (simplified for subsequent calculations).
Eigen::MatrixXf SI;
// Identity matrix of size [dim_x,dim_x], used for convenient calculations. This cannot be changed.
const Eigen::MatrixXf I = Eigen::MatrixXf::Identity(dim_x, dim_x);
// There will always be a copy of x, P after predict() is called.
// If there is a need to assign values between two Eigen matrices, the precondition is that they should be initialized properly, as this ensures that the number of columns and rows are compatible.
Eigen::VectorXf x_prior;
Eigen::MatrixXf P_prior;
// there will always be a copy of x,P after update() is called
Eigen::VectorXf x_post;
Eigen::MatrixXf P_post;
// keeps all observations. When there is a 'z', it is directly pushed back.
std::vector<Eigen::VectorXf*> history_obs;
// The following is newly added by ocsort.
// Used to mark the tracking state (whether there is still a target matching this trajectory), default value is false.
bool observed = false;
std::vector<Eigen::VectorXf*> new_history; // Used to create a virtual trajectory.
/* todo: Let's change the way we store variables, as C++ does not have Python's self.dict.
Using map<string,any> incurs high memory overhead, and there are errors when assigning values to Eigen data.
Someone in the group suggested using metadata to achieve this, but I don't know how to do it.
Therefore, here we will use a structure to save variables.
*/
struct Data {
Eigen::VectorXf x;
Eigen::MatrixXf P;
Eigen::MatrixXf Q;
Eigen::MatrixXf B;
Eigen::MatrixXf F;
Eigen::MatrixXf H;
Eigen::MatrixXf R;
float _alpha_sq = 1.;
Eigen::MatrixXf M;
Eigen::VectorXf z;
Eigen::MatrixXf K;
Eigen::MatrixXf y;
Eigen::MatrixXf S;
Eigen::MatrixXf SI;
Eigen::VectorXf x_prior;
Eigen::MatrixXf P_prior;
Eigen::VectorXf x_post;
Eigen::MatrixXf P_post;
std::vector<Eigen::VectorXf*> history_obs;
bool observed = false;
// The following is to determine whether the data has been saved due to freezing.
bool IsInitialized = false;
};
struct Data attr_saved;
};
}
#endif

24
ANSMOT/OCSort/include/OCSortObject.h Normal file
View File

@@ -0,0 +1,24 @@
#pragma once
#include "OCSortRect.h"
namespace ANSOCSort
{
struct Object
{
Rect<float> rect;
int label;
float prob;
float left;
float top;
float right;
float bottom;
std::string object_id;
Object(const Rect<float>& _rect,
const int& _label,
const float& _prob, const float& _left,
const float& _top,
const float& _right,
const float& _bottom,
const std::string& _object_id);
};
}

51
ANSMOT/OCSort/include/OCSortRect.h Normal file
View File

@@ -0,0 +1,51 @@
#pragma once
#include "Eigen/Dense"
namespace ANSOCSort
{
template<typename T>
using Tlwh = Eigen::Matrix<T, 1, 4, Eigen::RowMajor>;
template<typename T>
using Tlbr = Eigen::Matrix<T, 1, 4, Eigen::RowMajor>;
template<typename T>
using Xyah = Eigen::Matrix<T, 1, 4, Eigen::RowMajor>;
template<typename T>
class Rect
{
public:
Tlwh<T> tlwh;
Rect() = default;
Rect(const T& x, const T& y, const T& width, const T& height);
~Rect();
const T& x() const;
const T& y() const;
const T& width() const;
const T& height() const;
T& x();
T& y();
T& width();
T& height();
const T& tl_x() const;
const T& tl_y() const;
T br_x() const;
T br_y() const;
Tlbr<T> getTlbr() const;
Xyah<T> getXyah() const;
float calcIoU(const Rect<T>& other) const;
};
template<typename T>
Rect<T> generate_rect_by_tlbr(const Tlbr<T>& tlbr);
template<typename T>
Rect<T> generate_rect_by_xyah(const Xyah<T>& xyah);
}

18
ANSMOT/OCSort/include/OCSortUtilities.h Normal file
View File

@@ -0,0 +1,18 @@
#ifndef OCSORTUTILITIES_H
#define OCSORTUTILITIES_H
#include "Eigen/Dense"
#include <unordered_map>
namespace ANSOCSort {
/**
* Takes a bounding box in the form [x1,y1,x2,y2] and returns z in the form
[x,y,s,r] where x,y is the centre of the box and s is the scale/area and r is
the aspect ratio
* @param bbox
* @return z
*/
Eigen::VectorXf convert_bbox_to_z(Eigen::VectorXf bbox);
Eigen::VectorXf speed_direction(Eigen::VectorXf bbox1, Eigen::VectorXf bbox2);
Eigen::VectorXf convert_x_to_bbox(Eigen::VectorXf x);
Eigen::VectorXf k_previous_obs(std::unordered_map<int, Eigen::VectorXf> observations_, int cur_age, int k);
}
#endif

82
ANSMOT/OCSort/include/OCSortlapjv.h Normal file
View File

@@ -0,0 +1,82 @@
#ifndef OCSORTLAPJV_H
#define OCSORTLAPJV_H
#include "vector"
#define LARGE 1000000
#if !defined TRUE
#define TRUE 1
#endif
#if !defined FALSE
#define FALSE 0
#endif
#define NEW(x, t, n) \
if ((x = (t *) malloc(sizeof(t) * (n))) == 0) { return -1; }
#define FREE(x) \
if (x != 0) { \
free(x); \
x = 0; \
}
#define SWAP_INDICES(a, b) \
{ \
int_t _temp_index = a; \
a = b; \
b = _temp_index; \
}
#if 0
#include <assert.h>
#define ASSERT(cond) assert(cond)
#define PRINTF(fmt, ...) printf(fmt, ##__VA_ARGS__)
#define PRINT_COST_ARRAY(a, n) \
while (1) { \
printf(#a " = ["); \
if ((n) > 0) { \
printf("%f", (a)[0]); \
for (uint_t j = 1; j < n; j++) { \
printf(", %f", (a)[j]); \
} \
} \
printf("]\n"); \
break; \
}
#define PRINT_INDEX_ARRAY(a, n) \
while (1) { \
printf(#a " = ["); \
if ((n) > 0) { \
printf("%d", (a)[0]); \
for (uint_t j = 1; j < n; j++) { \
printf(", %d", (a)[j]); \
} \
} \
printf("]\n"); \
break; \
}
#else
#define ASSERT(cond)
#define PRINTF(fmt, ...)
#define PRINT_COST_ARRAY(a, n)
#define PRINT_INDEX_ARRAY(a, n)
#endif
namespace ANSOCSort {
typedef signed int int_t;
typedef unsigned int uint_t;
typedef float cost_t;
typedef char boolean;
typedef enum fp_t {
FP_1 = 1,
FP_2 = 2,
FP_DYNAMIC = 3
} fp_t;
extern int_t lapjv_internal(
const uint_t n, cost_t* cost[],
int_t* x, int_t* y);
extern int_t lapmod_internal(
const uint_t n, cost_t* cc, uint_t* ii, uint_t* kk,
int_t* x, int_t* y, fp_t fp_version);
float execLapjv(const std::vector<std::vector<float>>& cost, std::vector<int>& rowsol,
std::vector<int>& colsol, bool extend_cost, float cost_limit, bool return_cost);
}
#endif

190
ANSMOT/OCSort/src/Association.cpp Normal file
View File

@@ -0,0 +1,190 @@
#include "Association.h"
#include <iomanip>
#include <iostream>
namespace ANSOCSort {
std::tuple<Eigen::MatrixXf, Eigen::MatrixXf> speed_direction_batch(const Eigen::MatrixXf& dets,
const Eigen::MatrixXf& tracks) {
Eigen::VectorXf CX1 = (dets.col(0) + dets.col(2)) / 2.0;
Eigen::VectorXf CY1 = (dets.col(1) + dets.col(3)) / 2.f;
Eigen::MatrixXf CX2 = (tracks.col(0) + tracks.col(2)) / 2.f;
Eigen::MatrixXf CY2 = (tracks.col(1) + tracks.col(3)) / 2.f;
Eigen::MatrixXf dx = CX1.transpose().replicate(tracks.rows(), 1) - CX2.replicate(1, dets.rows());
Eigen::MatrixXf dy = CY1.transpose().replicate(tracks.rows(), 1) - CY2.replicate(1, dets.rows());
Eigen::MatrixXf norm = (dx.array().square() + dy.array().square()).sqrt() + 1e-6f;
dx = dx.array() / norm.array();
dy = dy.array() / norm.array();
return std::make_tuple(dy, dx);
}
Eigen::MatrixXf iou_batch(const Eigen::MatrixXf& bboxes1, const Eigen::MatrixXf& bboxes2) {
Eigen::Matrix<float, Eigen::Dynamic, 1> a = bboxes1.col(0);// bboxes1[..., 0] (n1,1)
Eigen::Matrix<float, 1, Eigen::Dynamic> b = bboxes2.col(0);// bboxes2[..., 0] (1,n2)
Eigen::MatrixXf xx1 = (a.replicate(1, b.cols())).cwiseMax(b.replicate(a.rows(), 1));
a = bboxes1.col(1);// bboxes1[..., 1]
b = bboxes2.col(1);// bboxes2[..., 1]
Eigen::MatrixXf yy1 = (a.replicate(1, b.cols())).cwiseMax(b.replicate(a.rows(), 1));
a = bboxes1.col(2);// bboxes1[..., 2]
b = bboxes2.col(2);// bboxes1[..., 2]
Eigen::MatrixXf xx2 = (a.replicate(1, b.cols())).cwiseMin(b.replicate(a.rows(), 1));
a = bboxes1.col(3);// bboxes1[..., 3]
b = bboxes2.col(3);// bboxes1[..., 3]
Eigen::MatrixXf yy2 = (a.replicate(1, b.cols())).cwiseMin(b.replicate(a.rows(), 1));
Eigen::MatrixXf w = (xx2 - xx1).cwiseMax(0);
Eigen::MatrixXf h = (yy2 - yy1).cwiseMax(0);
Eigen::MatrixXf wh = w.array() * h.array();
a = (bboxes1.col(2) - bboxes1.col(0)).array() * (bboxes1.col(3) - bboxes1.col(1)).array();
b = (bboxes2.col(2) - bboxes2.col(0)).array() * (bboxes2.col(3) - bboxes2.col(1)).array();
Eigen::MatrixXf part1_ = a.replicate(1, b.cols());
Eigen::MatrixXf part2_ = b.replicate(a.rows(), 1);
Eigen::MatrixXf Sum = (part1_ + part2_ - wh).cwiseMax(1e-7f);
return wh.cwiseQuotient(Sum);
}
Eigen::MatrixXf giou_batch(const Eigen::MatrixXf& bboxes1, const Eigen::MatrixXf& bboxes2) {
Eigen::Matrix<float, Eigen::Dynamic, 1> a = bboxes1.col(0);// bboxes1[..., 0] (n1,1)
Eigen::Matrix<float, 1, Eigen::Dynamic> b = bboxes2.col(0);// bboxes2[..., 0] (1,n2)
Eigen::MatrixXf xx1 = (a.replicate(1, b.cols())).cwiseMax(b.replicate(a.rows(), 1));
a = bboxes1.col(1);// bboxes1[..., 1]
b = bboxes2.col(1);// bboxes2[..., 1]
Eigen::MatrixXf yy1 = (a.replicate(1, b.cols())).cwiseMax(b.replicate(a.rows(), 1));
a = bboxes1.col(2);// bboxes1[..., 2]
b = bboxes2.col(2);// bboxes1[..., 2]
Eigen::MatrixXf xx2 = (a.replicate(1, b.cols())).cwiseMin(b.replicate(a.rows(), 1));
a = bboxes1.col(3);// bboxes1[..., 3]
b = bboxes2.col(3);// bboxes1[..., 3]
Eigen::MatrixXf yy2 = (a.replicate(1, b.cols())).cwiseMin(b.replicate(a.rows(), 1));
Eigen::MatrixXf w = (xx2 - xx1).cwiseMax(0);
Eigen::MatrixXf h = (yy2 - yy1).cwiseMax(0);
Eigen::MatrixXf wh = w.array() * h.array();
a = (bboxes1.col(2) - bboxes1.col(0)).array() * (bboxes1.col(3) - bboxes1.col(1)).array();
b = (bboxes2.col(2) - bboxes2.col(0)).array() * (bboxes2.col(3) - bboxes2.col(1)).array();
Eigen::MatrixXf part1_ = a.replicate(1, b.cols());
Eigen::MatrixXf part2_ = b.replicate(a.rows(), 1);
Eigen::MatrixXf Sum = (part1_ + part2_ - wh).cwiseMax(1e-7f);
Eigen::MatrixXf iou = wh.cwiseQuotient(Sum);
a = bboxes1.col(0);
b = bboxes2.col(0);
Eigen::MatrixXf xxc1 = (a.replicate(1, b.cols())).cwiseMin(b.replicate(a.rows(), 1));
a = bboxes1.col(1);// bboxes1[..., 1]
b = bboxes2.col(1);// bboxes2[..., 1]
Eigen::MatrixXf yyc1 = (a.replicate(1, b.cols())).cwiseMin(b.replicate(a.rows(), 1));
a = bboxes1.col(2);// bboxes1[..., 2]
b = bboxes2.col(2);// bboxes1[..., 2]
Eigen::MatrixXf xxc2 = (a.replicate(1, b.cols())).cwiseMax(b.replicate(a.rows(), 1));
a = bboxes1.col(3);// bboxes1[..., 3]
b = bboxes2.col(3);// bboxes1[..., 3]
Eigen::MatrixXf yyc2 = (a.replicate(1, b.cols())).cwiseMax(b.replicate(a.rows(), 1));
Eigen::MatrixXf wc = xxc2 - xxc1;
Eigen::MatrixXf hc = yyc2 - yyc1;
if ((wc.array() > 0).all() && (hc.array() > 0).all())
return iou;
else {
Eigen::MatrixXf area_enclose = (wc.array() * hc.array()).cwiseMax(1e-7f);
Eigen::MatrixXf giou = iou.array() - (area_enclose.array() - wh.array()) / area_enclose.array();
giou = (giou.array() + 1) / 2.0;
return giou;
}
}
std::tuple<std::vector<Eigen::Matrix<int, 1, 2>>, std::vector<int>, std::vector<int>> associate(Eigen::MatrixXf detections, Eigen::MatrixXf trackers, float iou_threshold, Eigen::MatrixXf velocities, Eigen::MatrixXf previous_obs_, float vdc_weight) {
if (trackers.rows() == 0) {
std::vector<int> unmatched_dets;
for (int i = 0; i < detections.rows(); i++) {
unmatched_dets.push_back(i);
}
return std::make_tuple(std::vector<Eigen::Matrix<int, 1, 2>>(), unmatched_dets, std::vector<int>());
}
Eigen::MatrixXf Y, X;
auto result = speed_direction_batch(detections, previous_obs_);
Y = std::get<0>(result);
X = std::get<1>(result);
Eigen::MatrixXf inertia_Y = velocities.col(0);
Eigen::MatrixXf inertia_X = velocities.col(1);
Eigen::MatrixXf inertia_Y_ = inertia_Y.replicate(1, Y.cols());
Eigen::MatrixXf inertia_X_ = inertia_X.replicate(1, X.cols());
Eigen::MatrixXf diff_angle_cos = inertia_X_.array() * X.array() + inertia_Y_.array() * Y.array();
diff_angle_cos = (diff_angle_cos.array().min(1).max(-1)).matrix();
Eigen::MatrixXf diff_angle = Eigen::acos(diff_angle_cos.array());
diff_angle = (m_pi / 2.0 - diff_angle.array().abs()).array() / (m_pi);
Eigen::Array<bool, 1, Eigen::Dynamic> valid_mask = Eigen::Array<bool, Eigen::Dynamic, 1>::Ones(previous_obs_.rows());
valid_mask = valid_mask.array() * ((previous_obs_.col(4).array() >= 0).transpose()).array();
Eigen::MatrixXf iou_matrix = iou_batch(detections, trackers);
Eigen::MatrixXf scores = detections.col(detections.cols() - 2).replicate(1, trackers.rows());
Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> valid_mask_ = (valid_mask.transpose()).replicate(1, X.cols());
Eigen::MatrixXf angle_diff_cost = ((((valid_mask_.cast<float>()).array() * diff_angle.array()).array() * vdc_weight)
.transpose())
.array() *
scores.array();
Eigen::Matrix<float, Eigen::Dynamic, 2> matched_indices(0, 2);
if (std::min(iou_matrix.cols(), iou_matrix.rows()) > 0) {
Eigen::MatrixXf a = (iou_matrix.array() > iou_threshold).cast<float>();
float sum1 = (a.rowwise().sum()).maxCoeff();
float sum0 = (a.colwise().sum()).maxCoeff();
if ((fabs(sum1 - 1) < 1e-12) && (fabs(sum0 - 1) < 1e-12)) {
for (int i = 0; i < a.rows(); i++) {
for (int j = 0; j < a.cols(); j++) {
if (a(i, j) > 0) {
Eigen::RowVectorXf row(2);
row << i, j;
matched_indices.conservativeResize(matched_indices.rows() + 1, Eigen::NoChange);
matched_indices.row(matched_indices.rows() - 1) = row;
}
}
}
}
else {
Eigen::MatrixXf cost_matrix = iou_matrix.array() + angle_diff_cost.array();
std::vector<std::vector<float>> cost_iou_matrix(cost_matrix.rows(), std::vector<float>(cost_matrix.cols()));
for (int i = 0; i < cost_matrix.rows(); i++) {
for (int j = 0; j < cost_matrix.cols(); j++) {
cost_iou_matrix[i][j] = -cost_matrix(i, j);
}
}
std::vector<int> rowsol, colsol;
float MIN_cost = execLapjv(cost_iou_matrix, rowsol, colsol, true, 0.01, true);
for (int i = 0; i < rowsol.size(); i++) {
if (rowsol.at(i) >= 0) {
Eigen::RowVectorXf row(2);
row << colsol.at(rowsol.at(i)), rowsol.at(i);
matched_indices.conservativeResize(matched_indices.rows() + 1, Eigen::NoChange);
matched_indices.row(matched_indices.rows() - 1) = row;
}
}
}
}
else {
matched_indices = Eigen::MatrixXf(0, 2);
}
std::vector<int> unmatched_detections;
for (int i = 0; i < detections.rows(); i++) {
if ((matched_indices.col(0).array() == i).sum() == 0) {
unmatched_detections.push_back(i);
}
}
std::vector<int> unmatched_trackers;
for (int i = 0; i < trackers.rows(); i++) {
if ((matched_indices.col(1).array() == i).sum() == 0) {
unmatched_trackers.push_back(i);
}
}
std::vector<Eigen::Matrix<int, 1, 2>> matches;
Eigen::Matrix<int, 1, 2> tmp;
for (int i = 0; i < matched_indices.rows(); i++) {
tmp = (matched_indices.row(i)).cast<int>();
if (iou_matrix(tmp(0), tmp(1)) < iou_threshold) {
unmatched_detections.push_back(tmp(0));
unmatched_trackers.push_back(tmp(1));
}
else {
matches.push_back(tmp);
}
}
if (matches.size() == 0) {
matches.clear();
}
return std::make_tuple(matches, unmatched_detections, unmatched_trackers);
}
}// namespace ocsort

168
ANSMOT/OCSort/src/KalmanBoxTracker.cpp Normal file
View File

@@ -0,0 +1,168 @@
#include <utility>
#include "KalmanBoxTracker.h"
namespace ANSOCSort {
int KalmanBoxTracker::count = 0;
KalmanBoxTracker::~KalmanBoxTracker() {
if (kf) {
delete kf;
kf = nullptr;
}
}
KalmanBoxTracker::KalmanBoxTracker(const KalmanBoxTracker& other)
: bbox(other.bbox),
kf(other.kf ? new KalmanFilterNew(*other.kf) : nullptr),
time_since_update(other.time_since_update),
id(other.id),
history(other.history),
hits(other.hits),
hit_streak(other.hit_streak),
age(other.age),
last_observation(other.last_observation),
observations(other.observations),
history_observations(other.history_observations),
velocity(other.velocity),
delta_t(other.delta_t)
{
}
KalmanBoxTracker& KalmanBoxTracker::operator=(const KalmanBoxTracker& other) {
if (this != &other) {
delete kf;
kf = other.kf ? new KalmanFilterNew(*other.kf) : nullptr;
bbox = other.bbox;
time_since_update = other.time_since_update;
id = other.id;
history = other.history;
hits = other.hits;
hit_streak = other.hit_streak;
age = other.age;
last_observation = other.last_observation;
observations = other.observations;
history_observations = other.history_observations;
velocity = other.velocity;
delta_t = other.delta_t;
}
return *this;
}
KalmanBoxTracker::KalmanBoxTracker(KalmanBoxTracker&& other) noexcept
: bbox(std::move(other.bbox)),
kf(other.kf),
time_since_update(other.time_since_update),
id(other.id),
history(std::move(other.history)),
hits(other.hits),
hit_streak(other.hit_streak),
age(other.age),
last_observation(std::move(other.last_observation)),
observations(std::move(other.observations)),
history_observations(std::move(other.history_observations)),
velocity(std::move(other.velocity)),
delta_t(other.delta_t)
{
other.kf = nullptr;
}
KalmanBoxTracker& KalmanBoxTracker::operator=(KalmanBoxTracker&& other) noexcept {
if (this != &other) {
delete kf;
bbox = std::move(other.bbox);
kf = other.kf;
other.kf = nullptr;
time_since_update = other.time_since_update;
id = other.id;
history = std::move(other.history);
hits = other.hits;
hit_streak = other.hit_streak;
age = other.age;
last_observation = std::move(other.last_observation);
observations = std::move(other.observations);
history_observations = std::move(other.history_observations);
velocity = std::move(other.velocity);
delta_t = other.delta_t;
}
return *this;
}
KalmanBoxTracker::KalmanBoxTracker(Eigen::VectorXf bbox_, int delta_t_) {
bbox = std::move(bbox_);
delta_t = delta_t_;
kf = new KalmanFilterNew(7, 4);//(7;4)
kf->F << 1, 0, 0, 0, 1, 0, 0,
0, 1, 0, 0, 0, 1, 0,
0, 0, 1, 0, 0, 0, 1,
0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 0, 1;
kf->H << 1, 0, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0;
kf->R.block(2, 2, 2, 2) *= 10.0;
kf->P.block(4, 4, 3, 3) *= 1000.0;
kf->P *= 10.0;
kf->Q.bottomRightCorner(1, 1)(0, 0) *= 0.01;
kf->Q.block(4, 4, 3, 3) *= 0.01;
kf->x.head<4>() = convert_bbox_to_z(bbox);
time_since_update = 0;
id = KalmanBoxTracker::count;
KalmanBoxTracker::count += 1;
history.clear();
hits = 0;
hit_streak = 0;
age = 0;
last_observation.fill(-1);
observations.clear();
history_observations.clear();
velocity.fill(0);
}
void KalmanBoxTracker::update(Eigen::Matrix<float, 9, 1>* bbox_) {
if (bbox_ != nullptr) {
if (int(last_observation.sum()) >= 0) {
Eigen::VectorXf previous_box_tmp;
for (int i = 0; i < delta_t; ++i) {
int dt = delta_t - i;
if (observations.count(age - dt) > 0) {
previous_box_tmp = observations[age - dt];
break;
}
}
if (0 == previous_box_tmp.size()) {
previous_box_tmp = last_observation;
}
velocity = speed_direction(previous_box_tmp, *bbox_);
}
last_observation = *bbox_;
observations[age] = *bbox_;
history_observations.push_back(*bbox_);
time_since_update = 0;
history.clear();
hits += 1;
hit_streak += 1;
Eigen::VectorXf tmp = convert_bbox_to_z(*bbox_);
kf->update(&tmp);
}
else {
kf->update(nullptr);
}
}
Eigen::RowVectorXf KalmanBoxTracker::predict() {
if (kf->x[6] + kf->x[2] <= 0) kf->x[6] *= 0.0;
kf->predict();
age += 1;
if (time_since_update > 0) hit_streak = 0;
time_since_update += 1;
history.push_back(convert_x_to_bbox(kf->x));
return convert_x_to_bbox(kf->x);
}
Eigen::VectorXf KalmanBoxTracker::get_state() {
return convert_x_to_bbox(kf->x);
}
}// namespace ocsort

302
ANSMOT/OCSort/src/OCSort.cpp Normal file
View File

@@ -0,0 +1,302 @@
#include "OCSort.h"
#include "iomanip"
#include <utility>
namespace ANSOCSort {
template<typename Matrix>
std::ostream& operator<<(std::ostream& os, const std::vector<Matrix>& v) {
os << "{";
for (auto it = v.begin(); it != v.end(); ++it) {
os << "(" << *it << ")\n";
if (it != v.end() - 1) os << ",";
}
os << "}\n";
return os;
}
OCSort::OCSort() {
update_parameters(0, 50, 1, 0.22136877277096445, 1, "giou", 0.3941737016672115, true);
}
void OCSort::update_parameters(float det_thresh_, int max_age_, int min_hits_, float iou_threshold_, int delta_t_, std::string asso_func_, float inertia_, bool use_byte_, bool autoFrameRate) {
max_age = max_age_;
min_hits = min_hits_;
iou_threshold = iou_threshold_;
trackers.clear();
frame_count = 0;
det_thresh = det_thresh_;
delta_t = delta_t_;
std::unordered_map<std::string, std::function<Eigen::MatrixXf(const Eigen::MatrixXf&, const Eigen::MatrixXf&)>> ASSO_FUNCS{
{"iou", iou_batch},
{ "giou", giou_batch }};
;
std::function<Eigen::MatrixXf(const Eigen::MatrixXf&, const Eigen::MatrixXf&)> asso_func = ASSO_FUNCS[asso_func_];
inertia = inertia_;
use_byte = use_byte_;
KalmanBoxTracker::count = 0;
// Frame rate auto-estimation
auto_frame_rate_ = autoFrameRate;
estimated_fps_ = 30.0f;
time_scale_factor_ = 1.0f;
fps_sample_count_ = 0;
has_last_update_time_ = false;
}
float OCSort::getEstimatedFps() const {
return estimated_fps_;
}
void OCSort::estimateFrameRate() {
if (!auto_frame_rate_) return;
auto now = std::chrono::steady_clock::now();
if (!has_last_update_time_) {
last_update_time_ = now;
has_last_update_time_ = true;
return;
}
double delta_sec = std::chrono::duration<double>(now - last_update_time_).count();
last_update_time_ = now;
if (delta_sec <= 0.001) return; // Skip near-zero intervals
float instant_fps = static_cast<float>(1.0 / delta_sec);
// EMA smoothing: alpha=0.3 for warmup (first 10 samples), alpha=0.1 steady state
fps_sample_count_++;
float alpha = (fps_sample_count_ <= 10) ? 0.3f : 0.1f;
estimated_fps_ = alpha * instant_fps + (1.0f - alpha) * estimated_fps_;
// Clamp estimated fps to a reasonable range
estimated_fps_ = std::max(1.0f, std::min(estimated_fps_, 120.0f));
// Compute time scale factor: ratio of actual interval to expected interval
float expected_dt = 1.0f / estimated_fps_;
time_scale_factor_ = static_cast<float>(delta_sec) / expected_dt;
time_scale_factor_ = std::max(0.5f, std::min(time_scale_factor_, 10.0f));
}
std::ostream& precision(std::ostream& os) {
os << std::fixed << std::setprecision(2);
return os;
}
std::vector<Eigen::RowVectorXf> OCSort::update(Eigen::MatrixXf dets) {
// Estimate frame rate from timing of update() calls
estimateFrameRate();
frame_count += 1;
std::vector<Eigen::RowVectorXf> ret;
try {
Eigen::Matrix<float, 1, Eigen::Dynamic> confs = dets.col(4);
Eigen::MatrixXf output_results = dets;
auto inds_low = confs.array() > 0.1;
auto inds_high = confs.array() < det_thresh;
auto inds_second = inds_low && inds_high;
Eigen::Matrix<float, Eigen::Dynamic, 9> dets_second;
Eigen::Matrix<bool, 1, Eigen::Dynamic> remain_inds = (confs.array() > det_thresh);
Eigen::Matrix<float, Eigen::Dynamic, 9> dets_first;
for (int i = 0; i < output_results.rows(); i++) {
if (true == inds_second(i)) {
dets_second.conservativeResize(dets_second.rows() + 1, Eigen::NoChange);
dets_second.row(dets_second.rows() - 1) = output_results.row(i);
}
if (true == remain_inds(i)) {
dets_first.conservativeResize(dets_first.rows() + 1, Eigen::NoChange);
dets_first.row(dets_first.rows() - 1) = output_results.row(i);
}
}
// Multi-predict: call predict() multiple times when frames are skipped
int num_predicts = 1;
if (auto_frame_rate_ && time_scale_factor_ > 1.5f) {
num_predicts = std::min(static_cast<int>(std::round(time_scale_factor_)), 10);
}
Eigen::MatrixXf trks = Eigen::MatrixXf::Zero(trackers.size(), 5);
std::vector<int> to_del;
for (int i = 0; i < trks.rows(); i++) {
// First predict call captures the return value for trks
Eigen::RowVectorXf pos = trackers[i].predict();
// Additional predict calls for frame skip compensation
for (int p = 1; p < num_predicts; p++) {
pos = trackers[i].predict();
}
trks.row(i) << pos(0), pos(1), pos(2), pos(3), 0;
}
// Adaptive matching: relax iou_threshold during frame skips
float effective_iou_threshold = iou_threshold;
if (num_predicts > 1) {
effective_iou_threshold = std::max(iou_threshold - 0.02f * (num_predicts - 1), 0.05f);
}
Eigen::MatrixXf velocities = Eigen::MatrixXf::Zero(trackers.size(), 2);
Eigen::MatrixXf last_boxes = Eigen::MatrixXf::Zero(trackers.size(), 9);
Eigen::MatrixXf k_observations = Eigen::MatrixXf::Zero(trackers.size(), 9);
for (int i = 0; i < trackers.size(); i++) {
velocities.row(i) = trackers[i].velocity;
last_boxes.row(i) = trackers[i].last_observation;
k_observations.row(i) = k_previous_obs(trackers[i].observations, trackers[i].age, delta_t);
}
std::vector<Eigen::Matrix<int, 1, 2>> matched;
std::vector<int> unmatched_dets;
std::vector<int> unmatched_trks;
auto result = associate(dets_first, trks, effective_iou_threshold, velocities, k_observations, inertia);
matched = std::get<0>(result);
unmatched_dets = std::get<1>(result);
unmatched_trks = std::get<2>(result);
for (auto m : matched) {
Eigen::Matrix<float, 9, 1> tmp_bbox;
tmp_bbox = dets_first.block<1, 9>(m(0), 0);
trackers[m(1)].update(&(tmp_bbox));
}
if (true == use_byte && dets_second.rows() > 0 && unmatched_trks.size() > 0) {
Eigen::MatrixXf u_trks(unmatched_trks.size(), trks.cols());
int index_for_u_trks = 0;
for (auto i : unmatched_trks) {
u_trks.row(index_for_u_trks++) = trks.row(i);
}
Eigen::MatrixXf iou_left = giou_batch(dets_second, u_trks);
if (iou_left.maxCoeff() > effective_iou_threshold) {
std::vector<std::vector<float>> iou_matrix(iou_left.rows(), std::vector<float>(iou_left.cols()));
for (int i = 0; i < iou_left.rows(); i++) {
for (int j = 0; j < iou_left.cols(); j++) {
iou_matrix[i][j] = -iou_left(i, j);
}
}
std::vector<int> rowsol, colsol;
float MIN_cost = execLapjv(iou_matrix, rowsol, colsol, true, 0.01, true);
std::vector<std::vector<int>> matched_indices;
for (int i = 0; i < rowsol.size(); i++) {
if (rowsol.at(i) >= 0) {
matched_indices.push_back({ colsol.at(rowsol.at(i)), rowsol.at(i) });
}
}
std::vector<int> to_remove_trk_indices;
for (auto m : matched_indices) {
int det_ind = m[0];
int trk_ind = unmatched_trks[m[1]];
if (iou_left(m[0], m[1]) < effective_iou_threshold) continue;
Eigen::Matrix<float, 9, 1> tmp_box;
tmp_box = dets_second.block<1, 9>(det_ind, 0);
trackers[trk_ind].update(&tmp_box);
to_remove_trk_indices.push_back(trk_ind);
}
std::vector<int> tmp_res1(unmatched_trks.size());
sort(unmatched_trks.begin(), unmatched_trks.end());
sort(to_remove_trk_indices.begin(), to_remove_trk_indices.end());
auto end1 = set_difference(unmatched_trks.begin(), unmatched_trks.end(),
to_remove_trk_indices.begin(), to_remove_trk_indices.end(),
tmp_res1.begin());
tmp_res1.resize(end1 - tmp_res1.begin());
unmatched_trks = tmp_res1;
}
}
if (unmatched_dets.size() > 0 && unmatched_trks.size() > 0) {
Eigen::MatrixXf left_dets(unmatched_dets.size(), 9);//6
int inx_for_dets = 0;
for (auto i : unmatched_dets) {
left_dets.row(inx_for_dets++) = dets_first.row(i);
}
Eigen::MatrixXf left_trks(unmatched_trks.size(), last_boxes.cols());
int indx_for_trk = 0;
for (auto i : unmatched_trks) {
left_trks.row(indx_for_trk++) = last_boxes.row(i);
}
Eigen::MatrixXf iou_left = giou_batch(left_dets, left_trks);
if (iou_left.maxCoeff() > effective_iou_threshold) {
std::vector<std::vector<float>> iou_matrix(iou_left.rows(), std::vector<float>(iou_left.cols()));
for (int i = 0; i < iou_left.rows(); i++) {
for (int j = 0; j < iou_left.cols(); j++) {
iou_matrix[i][j] = -iou_left(i, j);
}
}
std::vector<int> rowsol, colsol;
float MIN_cost = execLapjv(iou_matrix, rowsol, colsol, true, 0.01, true);
std::vector<std::vector<int>> rematched_indices;
for (int i = 0; i < rowsol.size(); i++) {
if (rowsol.at(i) >= 0) {
rematched_indices.push_back({ colsol.at(rowsol.at(i)), rowsol.at(i) });
}
}
std::vector<int> to_remove_det_indices;
std::vector<int> to_remove_trk_indices;
for (auto i : rematched_indices) {
int det_ind = unmatched_dets[i.at(0)];
int trk_ind = unmatched_trks[i.at(1)];
if (iou_left(i.at(0), i.at(1)) < effective_iou_threshold) {
continue;
}
Eigen::Matrix<float, 9, 1> tmp_bbox;
tmp_bbox = dets_first.block<1, 9>(det_ind, 0);
trackers.at(trk_ind).update(&tmp_bbox);
to_remove_det_indices.push_back(det_ind);
to_remove_trk_indices.push_back(trk_ind);
}
std::vector<int> tmp_res(unmatched_dets.size());
sort(unmatched_dets.begin(), unmatched_dets.end());
sort(to_remove_det_indices.begin(), to_remove_det_indices.end());
auto end = set_difference(unmatched_dets.begin(), unmatched_dets.end(),
to_remove_det_indices.begin(), to_remove_det_indices.end(),
tmp_res.begin());
tmp_res.resize(end - tmp_res.begin());
unmatched_dets = tmp_res;
std::vector<int> tmp_res1(unmatched_trks.size());
sort(unmatched_trks.begin(), unmatched_trks.end());
sort(to_remove_trk_indices.begin(), to_remove_trk_indices.end());
auto end1 = set_difference(unmatched_trks.begin(), unmatched_trks.end(),
to_remove_trk_indices.begin(), to_remove_trk_indices.end(),
tmp_res1.begin());
tmp_res1.resize(end1 - tmp_res1.begin());
unmatched_trks = tmp_res1;
}
}
for (auto m : unmatched_trks) {
trackers.at(m).update(nullptr);
}
for (int i : unmatched_dets) {
Eigen::RowVectorXf tmp_bbox = dets_first.block(i, 0, 1, 9);
KalmanBoxTracker trk(tmp_bbox, delta_t);
trackers.push_back(std::move(trk));
}
int tmp_i = trackers.size();
for (int i = trackers.size() - 1; i >= 0; i--) {
Eigen::Matrix<float, 1, 4> d;
int last_observation_sum = trackers.at(i).last_observation.sum();
if (last_observation_sum < 0) {
d = trackers.at(i).get_state();
}
else {
d = trackers.at(i).last_observation.block(0, 0, 1, 4);
}
if (trackers.at(i).time_since_update < 1 && ((trackers.at(i).hit_streak >= min_hits) | (frame_count <= min_hits))) {
Eigen::RowVectorXf tracking_res(10);
tracking_res << d(0), d(1), d(2), d(3),
trackers.at(i).id + 1, // Tracker Id
trackers.at(i).GetClassId(), // Class Id
trackers.at(i).GetScore(), // Score
trackers.at(i).GetDGId(), // DG Id
trackers.at(i).GetCamId(), // Camera Id
trackers.at(i).GetModelId(); // Model Id
ret.push_back(tracking_res);
}
if (trackers.at(i).time_since_update > max_age) {
trackers.erase(trackers.begin() + i);
}
}
return ret;
}
catch (const std::exception& e) {
return ret;
}
}
}//

155
ANSMOT/OCSort/src/OCSortKalmanFilter.cpp Normal file
View File

@@ -0,0 +1,155 @@
#include "OCSortKalmanFilter.h"
#include <iostream>
namespace ANSOCSort {
KalmanFilterNew::KalmanFilterNew() {};
KalmanFilterNew::KalmanFilterNew(int dim_x_, int dim_z_) {
dim_x = dim_x_;
dim_z = dim_z_;
x = Eigen::VectorXf::Zero(dim_x_, 1);
P = Eigen::MatrixXf::Identity(dim_x_, dim_x_);
Q = Eigen::MatrixXf::Identity(dim_x_, dim_x_);
B = Eigen::MatrixXf::Identity(dim_x_, dim_x_);
F = Eigen::MatrixXf::Identity(dim_x_, dim_x_);
H = Eigen::MatrixXf::Zero(dim_z_, dim_x_);
R = Eigen::MatrixXf::Identity(dim_z_, dim_z_);
M = Eigen::MatrixXf::Zero(dim_x_, dim_z_);
z = Eigen::VectorXf::Zero(dim_z_, 1);
K = Eigen::MatrixXf::Zero(dim_x_, dim_z_);
y = Eigen::VectorXf::Zero(dim_x_, 1);
S = Eigen::MatrixXf::Zero(dim_z_, dim_z_);
SI = Eigen::MatrixXf::Zero(dim_z_, dim_z_);
x_prior = x;
P_prior = P;
x_post = x;
P_post = P;
};
void KalmanFilterNew::predict() {
x = F * x;
P = _alpha_sq * ((F * P), F.transpose()) + Q;
x_prior = x;
P_prior = P;
}
void KalmanFilterNew::update(Eigen::VectorXf* z_) {
history_obs.push_back(z_);
if (z_ == nullptr) {
if (true == observed) freeze();
observed = false;
z = Eigen::VectorXf::Zero(dim_z, 1);
x_post = x;
P_post = P;
y = Eigen::VectorXf::Zero(dim_z, 1);
return;
}
if (false == observed) unfreeze();
observed = true;
y = *z_ - H * x;
auto PHT = P * H.transpose();
S = H * PHT + R;
K = PHT * SI;
x = x + K * y;
auto I_KH = I - K * H;
P = ((I_KH * P) * I_KH.transpose()) + ((K * R) * K.transpose());
z = *z_;
x_post = x;
P_post = P;
}
void KalmanFilterNew::freeze() {
attr_saved.IsInitialized = true;
attr_saved.x = x;
attr_saved.P = P;
attr_saved.Q = Q;
attr_saved.B = B;
attr_saved.F = F;
attr_saved.H = H;
attr_saved.R = R;
attr_saved._alpha_sq = _alpha_sq;
attr_saved.M = M;
attr_saved.z = z;
attr_saved.K = K;
attr_saved.y = y;
attr_saved.S = S;
attr_saved.SI = SI;
attr_saved.x_prior = x_prior;
attr_saved.P_prior = P_prior;
attr_saved.x_post = x_post;
attr_saved.P_post = P_post;
attr_saved.history_obs = history_obs;
}
void KalmanFilterNew::unfreeze() {
if (true == attr_saved.IsInitialized) {
new_history = history_obs;
x = attr_saved.x;
P = attr_saved.P;
Q = attr_saved.Q;
B = attr_saved.B;
F = attr_saved.F;
H = attr_saved.H;
R = attr_saved.R;
_alpha_sq = attr_saved._alpha_sq;
M = attr_saved.M;
z = attr_saved.z;
K = attr_saved.K;
y = attr_saved.y;
S = attr_saved.S;
SI = attr_saved.SI;
x_prior = attr_saved.x_prior;
P_prior = attr_saved.P_prior;
x_post = attr_saved.x_post;
history_obs.erase(history_obs.end() - 1);
Eigen::VectorXf box1;
Eigen::VectorXf box2;
int lastNotNullIndex = -1;
int secondLastNotNullIndex = -1;
for (int i = new_history.size() - 1; i >= 0; i--) {
if (new_history[i] != nullptr) {
if (lastNotNullIndex == -1) {
lastNotNullIndex = i;
box2 = *(new_history.at(lastNotNullIndex));
}
else if (secondLastNotNullIndex == -1) {
secondLastNotNullIndex = i;
box1 = *(new_history.at(secondLastNotNullIndex));
break;
}
}
}
double time_gap = lastNotNullIndex - secondLastNotNullIndex;
double x1 = box1[0];
double x2 = box2[0];
double y1 = box1[1];
double y2 = box2[1];
double w1 = std::sqrt(box1[2] * box1[3]);
double h1 = std::sqrt(box1[2] / box1[3]);
double w2 = std::sqrt(box1[2] * box1[3]);
double h2 = std::sqrt(box1[2] / box1[3]);
double dx = (x2 - x1) / time_gap;
double dy = (y1 - y2) / time_gap;
double dw = (w2 - w1) / time_gap;
double dh = (h2 - h1) / time_gap;
for (int i = 0; i < time_gap; i++) {
double x = x1 + (i + 1) * dx;
double y = y1 + (i + 1) * dy;
double w = w1 + (i + 1) * dw;
double h = h1 + (i + 1) * dh;
double s = w * h;
double r = w / (h * 1.0);
Eigen::VectorXf new_box(4, 1);
new_box << x, y, s, r;
this->y = new_box - this->H * this->x;
auto PHT = this->P * this->H.transpose();
this->S = this->H * PHT + this->R;
this->SI = (this->S).inverse();
this->K = PHT * this->SI;
this->x = this->x + this->K * this->y;
auto I_KH = this->I - this->K * this->H;
this->P = ((I_KH * this->P) * I_KH.transpose()) + ((this->K * this->R) * (this->K).transpose());
this->z = new_box;
this->x_post = this->x;
this->P_post = this->P;
if (i != (time_gap - 1)) predict();
}
}
}
}// namespace ocsort

19
ANSMOT/OCSort/src/OCSortObject.cpp Normal file
View File

@@ -0,0 +1,19 @@
#include "OCSortObject.h"
ANSOCSort::Object::Object(const ANSOCSort::Rect<float>& _rect,
const int& _label,
const float& _prob,
const float& _left,
const float& _top,
const float& _right,
const float& _bottom,
const std::string& _object_id) :
rect(_rect),
label(_label),
prob(_prob),
left(_left),
top(_top),
right(_right),
bottom(_bottom),
object_id(_object_id)
{
}

147
ANSMOT/OCSort/src/OCSortRect.cpp Normal file
View File

@@ -0,0 +1,147 @@
#include "OCSortRect.h"
#include <algorithm>
template <typename T>
ANSOCSort::Rect<T>::Rect(const T& x, const T& y, const T& width, const T& height) :
tlwh({ x, y, width, height })
{
}
template <typename T>
ANSOCSort::Rect<T>::~Rect()
{
}
template <typename T>
const T& ANSOCSort::Rect<T>::x() const
{
return tlwh[0];
}
template <typename T>
const T& ANSOCSort::Rect<T>::y() const
{
return tlwh[1];
}
template <typename T>
const T& ANSOCSort::Rect<T>::width() const
{
return tlwh[2];
}
template <typename T>
const T& ANSOCSort::Rect<T>::height() const
{
return tlwh[3];
}
template <typename T>
T& ANSOCSort::Rect<T>::x()
{
return tlwh[0];
}
template <typename T>
T& ANSOCSort::Rect<T>::y()
{
return tlwh[1];
}
template <typename T>
T& ANSOCSort::Rect<T>::width()
{
return tlwh[2];
}
template <typename T>
T& ANSOCSort::Rect<T>::height()
{
return tlwh[3];
}
template <typename T>
const T& ANSOCSort::Rect<T>::tl_x() const
{
return tlwh[0];
}
template <typename T>
const T& ANSOCSort::Rect<T>::tl_y() const
{
return tlwh[1];
}
template <typename T>
T ANSOCSort::Rect<T>::br_x() const
{
return tlwh[0] + tlwh[2];
}
template <typename T>
T ANSOCSort::Rect<T>::br_y() const
{
return tlwh[1] + tlwh[3];
}
template <typename T>
ANSOCSort::Tlbr<T> ANSOCSort::Rect<T>::getTlbr() const
{
return {
tlwh[0],
tlwh[1],
tlwh[0] + tlwh[2],
tlwh[1] + tlwh[3],
};
}
template <typename T>
ANSOCSort::Xyah<T> ANSOCSort::Rect<T>::getXyah() const
{
return {
tlwh[0] + tlwh[2] / 2,
tlwh[1] + tlwh[3] / 2,
tlwh[2] / tlwh[3],
tlwh[3],
};
}
template<typename T>
float ANSOCSort::Rect<T>::calcIoU(const Rect<T>& other) const
{
const float box_area = (other.tlwh[2] + 1) * (other.tlwh[3] + 1);
const float iw = std::min(tlwh[0] + tlwh[2], other.tlwh[0] + other.tlwh[2]) - std::max(tlwh[0], other.tlwh[0]) + 1;
float iou = 0;
if (iw > 0)
{
const float ih = std::min(tlwh[1] + tlwh[3], other.tlwh[1] + other.tlwh[3]) - std::max(tlwh[1], other.tlwh[1]) + 1;
if (ih > 0)
{
const float ua = (tlwh[0] + tlwh[2] - tlwh[0] + 1) * (tlwh[1] + tlwh[3] - tlwh[1] + 1) + box_area - iw * ih;
iou = iw * ih / ua;
}
}
return iou;
}
template<typename T>
ANSOCSort::Rect<T> ANSOCSort::generate_rect_by_tlbr(const ANSOCSort::Tlbr<T>& tlbr)
{
return ANSOCSort::Rect<T>(tlbr[0], tlbr[1], tlbr[2] - tlbr[0], tlbr[3] - tlbr[1]);
}
template<typename T>
ANSOCSort::Rect<T> ANSOCSort::generate_rect_by_xyah(const ANSOCSort::Xyah<T>& xyah)
{
const auto width = xyah[2] * xyah[3];
return ANSOCSort::Rect<T>(xyah[0] - width / 2, xyah[1] - xyah[3] / 2, width, xyah[3]);
}
// explicit instantiation
template class ANSOCSort::Rect<int>;
template class ANSOCSort::Rect<float>;
template ANSOCSort::Rect<int> ANSOCSort::generate_rect_by_tlbr<int>(const ANSOCSort::Tlbr<int>&);
template ANSOCSort::Rect<float> ANSOCSort::generate_rect_by_tlbr<float>(const ANSOCSort::Tlbr<float>&);
template ANSOCSort::Rect<int> ANSOCSort::generate_rect_by_xyah<int>(const ANSOCSort::Xyah<int>&);
template ANSOCSort::Rect<float> ANSOCSort::generate_rect_by_xyah<float>(const ANSOCSort::Xyah<float>&);

41
ANSMOT/OCSort/src/OCSortUtilities.cpp Normal file
View File

@@ -0,0 +1,41 @@
#include "OCSortUtilities.h"
namespace ANSOCSort {
Eigen::VectorXf convert_bbox_to_z(Eigen::VectorXf bbox) {
double w = bbox[2] - bbox[0];
double h = bbox[3] - bbox[1];
double x = bbox[0] + w / 2.0;
double y = bbox[1] + h / 2.0;
double s = w * h;
double r = w / (h + 1e-6);
Eigen::MatrixXf z(4, 1);
z << x, y, s, r;
return z;
}
Eigen::VectorXf speed_direction(Eigen::VectorXf bbox1, Eigen::VectorXf bbox2) {
double cx1 = (bbox1[0] + bbox1[2]) / 2.0;
double cy1 = (bbox1[1] + bbox1[3]) / 2.0;
double cx2 = (bbox2[0] + bbox2[2]) / 2.0;
double cy2 = (bbox2[1] + bbox2[3]) / 2.0;
Eigen::VectorXf speed(2, 1);
speed << cy2 - cy1, cx2 - cx1;
double norm = sqrt(pow(cy2 - cy1, 2) + pow(cx2 - cx1, 2)) + 1e-6;
return speed / norm;
}
Eigen::VectorXf convert_x_to_bbox(Eigen::VectorXf x) {
float w = std::sqrt(x(2) * x(3));
float h = x(2) / w;
Eigen::VectorXf bbox = Eigen::VectorXf::Ones(4, 1);
bbox << x(0) - w / 2, x(1) - h / 2, x(0) + w / 2, x(1) + h / 2;
return bbox;
}
Eigen::VectorXf k_previous_obs(std::unordered_map<int, Eigen::VectorXf> observations_, int cur_age, int k) {
if (observations_.size() == 0) return Eigen::VectorXf::Constant(5, -1.0);
for (int i = 0; i < k; i++) {
int dt = k - i;
if (observations_.count(cur_age - dt) > 0) return observations_.at(cur_age - dt);
}
auto iter = std::max_element(observations_.begin(), observations_.end(), [](const std::pair<int, Eigen::VectorXf>& p1, const std::pair<int, Eigen::VectorXf>& p2) { return p1.first < p2.first; });
int max_age = iter->first;
return observations_[max_age];
}
}// namespace ocsort

455
ANSMOT/OCSort/src/OCSortlapjv.cpp Normal file
View File

@@ -0,0 +1,455 @@
#include "OCSortlapjv.h"
#include <limits>
#include <stdexcept>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <vector>
namespace ANSOCSort {
int_t _ccrrt_dense(const uint_t n, cost_t* cost[],
int_t* free_rows, int_t* x, int_t* y, cost_t* v) {
int_t n_free_rows;
boolean* unique;
for (uint_t i = 0; i < n; i++) {
x[i] = -1;
v[i] = LARGE;
y[i] = 0;
}
for (uint_t i = 0; i < n; i++) {
for (uint_t j = 0; j < n; j++) {
const cost_t c = cost[i][j];
if (c < v[j]) {
v[j] = c;
y[j] = i;
}
PRINTF("i=%d, j=%d, c[i,j]=%f, v[j]=%f y[j]=%d\n", i, j, c, v[j], y[j]);
}
}
PRINT_COST_ARRAY(v, n);
PRINT_INDEX_ARRAY(y, n);
NEW(unique, boolean, n);
memset(unique, TRUE, n);
{
int_t j = n;
do {
j--;
const int_t i = y[j];
if (x[i] < 0) {
x[i] = j;
}
else {
unique[i] = FALSE;
y[j] = -1;
}
} while (j > 0);
}
n_free_rows = 0;
for (uint_t i = 0; i < n; i++) {
if (x[i] < 0) {
free_rows[n_free_rows++] = i;
}
else if (unique[i]) {
const int_t j = x[i];
cost_t min = LARGE;
for (uint_t j2 = 0; j2 < n; j2++) {
if (j2 == (uint_t)j) {
continue;
}
const cost_t c = cost[i][j2] - v[j2];
if (c < min) {
min = c;
}
}
PRINTF("v[%d] = %f - %f\n", j, v[j], min);
v[j] -= min;
}
}
FREE(unique);
return n_free_rows;
}
/** Augmenting row reduction for a dense cost matrix.
*/
int_t _carr_dense(
const uint_t n, cost_t* cost[],
const uint_t n_free_rows,
int_t* free_rows, int_t* x, int_t* y, cost_t* v) {
uint_t current = 0;
int_t new_free_rows = 0;
uint_t rr_cnt = 0;
PRINT_INDEX_ARRAY(x, n);
PRINT_INDEX_ARRAY(y, n);
PRINT_COST_ARRAY(v, n);
PRINT_INDEX_ARRAY(free_rows, n_free_rows);
while (current < n_free_rows) {
int_t i0;
int_t j1, j2;
cost_t v1, v2, v1_new;
boolean v1_lowers;
rr_cnt++;
PRINTF("current = %d rr_cnt = %d\n", current, rr_cnt);
const int_t free_i = free_rows[current++];
j1 = 0;
v1 = cost[free_i][0] - v[0];
j2 = -1;
v2 = LARGE;
for (uint_t j = 1; j < n; j++) {
PRINTF("%d = %f %d = %f\n", j1, v1, j2, v2);
const cost_t c = cost[free_i][j] - v[j];
if (c < v2) {
if (c >= v1) {
v2 = c;
j2 = j;
}
else {
v2 = v1;
v1 = c;
j2 = j1;
j1 = j;
}
}
}
i0 = y[j1];
v1_new = v[j1] - (v2 - v1);
v1_lowers = v1_new < v[j1];
PRINTF("%d %d 1=%d,%f 2=%d,%f v1'=%f(%d,%g) \n", free_i, i0, j1, v1, j2, v2, v1_new, v1_lowers, v[j1] - v1_new);
if (rr_cnt < current * n) {
if (v1_lowers) {
v[j1] = v1_new;
}
else if (i0 >= 0 && j2 >= 0) {
j1 = j2;
i0 = y[j2];
}
if (i0 >= 0) {
if (v1_lowers) {
free_rows[--current] = i0;
}
else {
free_rows[new_free_rows++] = i0;
}
}
}
else {
PRINTF("rr_cnt=%d >= %d (current=%d * n=%d)\n", rr_cnt, current * n, current, n);
if (i0 >= 0) {
free_rows[new_free_rows++] = i0;
}
}
x[free_i] = j1;
y[j1] = free_i;
}
return new_free_rows;
}
/** Find columns with minimum d[j] and put them on the SCAN list.
*/
uint_t _find_dense(const uint_t n, uint_t lo, cost_t* d, int_t* cols, int_t* y) {
uint_t hi = lo + 1;
cost_t mind = d[cols[lo]];
for (uint_t k = hi; k < n; k++) {
int_t j = cols[k];
if (d[j] <= mind) {
if (d[j] < mind) {
hi = lo;
mind = d[j];
}
cols[k] = cols[hi];
cols[hi++] = j;
}
}
return hi;
}
// Scan all columns in TODO starting from arbitrary column in SCAN
// and try to decrease d of the TODO columns using the SCAN column.
int_t _scan_dense(const uint_t n, cost_t* cost[],
uint_t* plo, uint_t* phi,
cost_t* d, int_t* cols, int_t* pred,
int_t* y, cost_t* v) {
uint_t lo = *plo;
uint_t hi = *phi;
cost_t h, cred_ij;
while (lo != hi) {
int_t j = cols[lo++];
const int_t i = y[j];
const cost_t mind = d[j];
h = cost[i][j] - v[j] - mind;
PRINTF("i=%d j=%d h=%f\n", i, j, h);
// For all columns in TODO
for (uint_t k = hi; k < n; k++) {
j = cols[k];
cred_ij = cost[i][j] - v[j] - h;
if (cred_ij < d[j]) {
d[j] = cred_ij;
pred[j] = i;
if (cred_ij == mind) {
if (y[j] < 0) {
return j;
}
cols[k] = cols[hi];
cols[hi++] = j;
}
}
}
}
*plo = lo;
*phi = hi;
return -1;
}
/** Single iteration of modified Dijkstra shortest path algorithm as explained in the JV paper.
*
* This is a dense matrix version.
*
* \return The closest free column index.
*/
int_t find_path_dense(
const uint_t n, cost_t* cost[],
const int_t start_i,
int_t* y, cost_t* v,
int_t* pred) {
uint_t lo = 0, hi = 0;
int_t final_j = -1;
uint_t n_ready = 0;
int_t* cols;
cost_t* d;
NEW(cols, int_t, n);
NEW(d, cost_t, n);
for (uint_t i = 0; i < n; i++) {
cols[i] = i;
pred[i] = start_i;
d[i] = cost[start_i][i] - v[i];
}
PRINT_COST_ARRAY(d, n);
while (final_j == -1) {
// No columns left on the SCAN list.
if (lo == hi) {
PRINTF("%d..%d -> find\n", lo, hi);
n_ready = lo;
hi = _find_dense(n, lo, d, cols, y);
PRINTF("check %d..%d\n", lo, hi);
PRINT_INDEX_ARRAY(cols, n);
for (uint_t k = lo; k < hi; k++) {
const int_t j = cols[k];
if (y[j] < 0) {
final_j = j;
}
}
}
if (final_j == -1) {
PRINTF("%d..%d -> scan\n", lo, hi);
final_j = _scan_dense(
n, cost, &lo, &hi, d, cols, pred, y, v);
PRINT_COST_ARRAY(d, n);
PRINT_INDEX_ARRAY(cols, n);
PRINT_INDEX_ARRAY(pred, n);
}
}
PRINTF("found final_j=%d\n", final_j);
PRINT_INDEX_ARRAY(cols, n);
{
const cost_t mind = d[cols[lo]];
for (uint_t k = 0; k < n_ready; k++) {
const int_t j = cols[k];
v[j] += d[j] - mind;
}
}
FREE(cols);
FREE(d);
return final_j;
}
/** Augment for a dense cost matrix.
*/
int_t _ca_dense(
const uint_t n, cost_t* cost[],
const uint_t n_free_rows,
int_t* free_rows, int_t* x, int_t* y, cost_t* v) {
int_t* pred;
NEW(pred, int_t, n);
for (int_t* pfree_i = free_rows; pfree_i < free_rows + n_free_rows; pfree_i++) {
int_t i = -1, j;
uint_t k = 0;
PRINTF("looking at free_i=%d\n", *pfree_i);
j = find_path_dense(n, cost, *pfree_i, y, v, pred);
ASSERT(j >= 0);
ASSERT(j < n);
while (i != *pfree_i) {
PRINTF("augment %d\n", j);
PRINT_INDEX_ARRAY(pred, n);
i = pred[j];
PRINTF("y[%d]=%d -> %d\n", j, y[j], i);
y[j] = i;
PRINT_INDEX_ARRAY(x, n);
SWAP_INDICES(j, x[i]);
k++;
if (k >= n) {
ASSERT(FALSE);
}
}
}
FREE(pred);
return 0;
}
/** Solve dense sparse LAP.
*/
int lapjv_internal(
const uint_t n, cost_t* cost[],
int_t* x, int_t* y) {
int ret;
int_t* free_rows;
cost_t* v;
NEW(free_rows, int_t, n);
NEW(v, cost_t, n);
ret = _ccrrt_dense(n, cost, free_rows, x, y, v);
int i = 0;
while (ret > 0 && i < 2) {
ret = _carr_dense(n, cost, ret, free_rows, x, y, v);
i++;
}
if (ret > 0) {
ret = _ca_dense(n, cost, ret, free_rows, x, y, v);
}
FREE(v);
FREE(free_rows);
return ret;
}
float execLapjv(const std::vector<std::vector<float>>& cost, std::vector<int>& rowsol,
std::vector<int>& colsol, bool extend_cost, float cost_limit, bool return_cost) {
std::vector<std::vector<float>> cost_c;
cost_c.assign(cost.begin(), cost.end());
std::vector<std::vector<float>> cost_c_extended;
int n_rows = cost.size();
int n_cols = cost[0].size();
rowsol.resize(n_rows);
colsol.resize(n_cols);
int n = 0;
if (n_rows == n_cols) {
n = n_rows;
}
else {
if (!extend_cost) {
throw std::runtime_error("The `extend_cost` variable should set True");
}
}
if (extend_cost || cost_limit < std::numeric_limits<float>::max()) {
n = n_rows + n_cols;
cost_c_extended.resize(n);
for (size_t i = 0; i < cost_c_extended.size(); i++)
cost_c_extended[i].resize(n);
if (cost_limit < std::numeric_limits<float>::max()) {
for (size_t i = 0; i < cost_c_extended.size(); i++) {
for (size_t j = 0; j < cost_c_extended[i].size(); j++) {
cost_c_extended[i][j] = cost_limit / 2.0;
}
}
}
else {
float cost_max = -1;
for (size_t i = 0; i < cost_c.size(); i++) {
for (size_t j = 0; j < cost_c[i].size(); j++) {
if (cost_c[i][j] > cost_max)
cost_max = cost_c[i][j];
}
}
for (size_t i = 0; i < cost_c_extended.size(); i++) {
for (size_t j = 0; j < cost_c_extended[i].size(); j++) {
cost_c_extended[i][j] = cost_max + 1;
}
}
}
for (size_t i = n_rows; i < cost_c_extended.size(); i++) {
for (size_t j = n_cols; j < cost_c_extended[i].size(); j++) {
cost_c_extended[i][j] = 0;
}
}
for (int i = 0; i < n_rows; i++) {
for (int j = 0; j < n_cols; j++) {
cost_c_extended[i][j] = cost_c[i][j];
}
}
cost_c.clear();
cost_c.assign(cost_c_extended.begin(), cost_c_extended.end());
}
float** cost_ptr;
cost_ptr = new float* [n];
for (int i = 0; i < n; i++)
cost_ptr[i] = new float[n];
for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++) {
cost_ptr[i][j] = cost_c[i][j];
}
}
int* x_c = new int[n];
int* y_c = new int[n];
int ret = lapjv_internal(n, cost_ptr, x_c, y_c);
if (ret != 0) {
for (int i = 0; i < n; i++) delete[] cost_ptr[i];
delete[] cost_ptr;
delete[] x_c;
delete[] y_c;
throw std::runtime_error("The result of lapjv_internal() is invalid.");
}
float opt = 0.0;
if (n != n_rows) {
for (int i = 0; i < n; i++) {
if (x_c[i] >= n_cols)
x_c[i] = -1;
if (y_c[i] >= n_rows)
y_c[i] = -1;
}
for (int i = 0; i < n_rows; i++) {
rowsol[i] = x_c[i];
}
for (int i = 0; i < n_cols; i++) {
colsol[i] = y_c[i];
}
if (return_cost) {
for (size_t i = 0; i < rowsol.size(); i++) {
if (rowsol[i] != -1) {
opt += cost_ptr[i][rowsol[i]];
}
}
}
}
else if (return_cost) {
for (size_t i = 0; i < rowsol.size(); i++) {
opt += cost_ptr[i][rowsol[i]];
}
}
for (int i = 0; i < n; i++) {
delete[] cost_ptr[i];
}
delete[] cost_ptr;
delete[] x_c;
delete[] y_c;
return opt;
}
}