Compare commits
No commits in common. "master" and "fix-ros" have entirely different histories.
|
|
@ -83,8 +83,6 @@ include_directories(
|
|||
${CMAKE_CURRENT_SOURCE_DIR}/algorithm/common_det/cuda
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/algorithm/landing_det/cuda
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/algorithm/sot/ocv470
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/algorithm/mot/sort
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/algorithm/mot/bytetrack
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/algorithm/color_line
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/video_io
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/algorithm/veri/cuda
|
||||
|
|
@ -184,10 +182,6 @@ file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/video_io/*.cpp)
|
|||
list(APPEND spirecv_SRCS ${ALG_SRC_FILES})
|
||||
file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/utils/*.cpp)
|
||||
list(APPEND spirecv_SRCS ${ALG_SRC_FILES})
|
||||
file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/mot/sort/*.cpp)
|
||||
list(APPEND spirecv_SRCS ${ALG_SRC_FILES})
|
||||
file(GLOB ALG_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/algorithm/mot/bytetrack/*.cpp)
|
||||
list(APPEND spirecv_SRCS ${ALG_SRC_FILES})
|
||||
|
||||
if(USE_CUDA)
|
||||
list(APPEND spirecv_SRCS algorithm/common_det/cuda/yolov7/preprocess.cu)
|
||||
|
|
@ -305,8 +299,6 @@ add_executable(SingleObjectTracking samples/demo/single_object_tracking.cpp)
|
|||
target_link_libraries(SingleObjectTracking sv_world)
|
||||
add_executable(MultipleObjectTracking samples/demo/multiple_object_tracking.cpp)
|
||||
target_link_libraries(MultipleObjectTracking sv_world)
|
||||
add_executable(EvalMOTMetric samples/demo/eval_MOT_metric.cpp)
|
||||
target_link_libraries(EvalMOTMetric -lstdc++fs sv_world)
|
||||
add_executable(ColorLineDetection samples/demo/color_line_detect.cpp)
|
||||
target_link_libraries(ColorLineDetection sv_world)
|
||||
add_executable(UdpDetectionInfoReceiver samples/demo/udp_detection_info_receiver.cpp)
|
||||
|
|
@ -372,12 +364,14 @@ if(PLATFORM STREQUAL "JETSON")
|
|||
file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/build/${PROJECT_NAME}Config.cmake.in [[
|
||||
@PACKAGE_INIT@
|
||||
find_package(OpenCV 4 REQUIRED)
|
||||
find_package(Eigen3 REQUIRED)
|
||||
link_directories(/usr/local/cuda/lib64)
|
||||
set(SV_INCLUDE_DIRS
|
||||
@SV_INSTALL_PREFIX@/include
|
||||
/usr/include/x86_64-linux-gnu
|
||||
/usr/local/cuda/include
|
||||
${OpenCV_INCLUDE_DIRS}
|
||||
${EIGEN3_INCLUDE_DIRS}
|
||||
/usr/include/gstreamer-1.0
|
||||
/usr/local/include/gstreamer-1.0
|
||||
/usr/include/glib-2.0
|
||||
|
|
@ -396,6 +390,7 @@ elseif(PLATFORM STREQUAL "X86_CUDA")
|
|||
file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/build/${PROJECT_NAME}Config.cmake.in [[
|
||||
@PACKAGE_INIT@
|
||||
find_package(OpenCV 4 REQUIRED)
|
||||
find_package(Eigen3 REQUIRED)
|
||||
find_package(fmt REQUIRED)
|
||||
link_directories(/usr/local/cuda/lib64)
|
||||
set(SV_INCLUDE_DIRS
|
||||
|
|
@ -403,6 +398,7 @@ set(SV_INCLUDE_DIRS
|
|||
/usr/include/x86_64-linux-gnu
|
||||
/usr/local/cuda/include
|
||||
${OpenCV_INCLUDE_DIRS}
|
||||
${EIGEN3_INCLUDE_DIRS}
|
||||
)
|
||||
set(SV_LIBRARIES
|
||||
@SV_INSTALL_PREFIX@/lib/libsv_yoloplugins.so
|
||||
|
|
@ -417,11 +413,13 @@ elseif(PLATFORM STREQUAL "X86_INTEL")
|
|||
file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/build/${PROJECT_NAME}Config.cmake.in [[
|
||||
@PACKAGE_INIT@
|
||||
find_package(OpenCV 4 REQUIRED)
|
||||
find_package(Eigen3 REQUIRED)
|
||||
find_package(fmt REQUIRED)
|
||||
set(SV_INCLUDE_DIRS
|
||||
@SV_INSTALL_PREFIX@/include
|
||||
/usr/include/x86_64-linux-gnu
|
||||
${OpenCV_INCLUDE_DIRS}
|
||||
${EIGEN3_INCLUDE_DIRS}
|
||||
)
|
||||
set(SV_LIBRARIES
|
||||
@SV_INSTALL_PREFIX@/lib/libsv_world.so
|
||||
|
|
|
|||
|
|
@ -1,262 +0,0 @@
|
|||
|
||||
#include "BYTETracker.h"
|
||||
#include <fstream>
|
||||
|
||||
namespace sv
|
||||
{
|
||||
|
||||
BYTETracker::BYTETracker(int frame_rate, int track_buffer)
|
||||
{
|
||||
track_thresh = 0.5;
|
||||
high_thresh = 0.6;
|
||||
match_thresh = 0.8;
|
||||
|
||||
frame_id = 0;
|
||||
max_time_lost = int(frame_rate / 30.0 * track_buffer);
|
||||
}
|
||||
|
||||
BYTETracker::~BYTETracker()
|
||||
{
|
||||
}
|
||||
|
||||
void BYTETracker::update(TargetsInFrame &tgts)
|
||||
{
|
||||
////////////////// Step 1: Get detections //////////////////
|
||||
this->frame_id++;
|
||||
std::vector<STrack> activated_stracks;
|
||||
std::vector<STrack> refind_stracks;
|
||||
std::vector<STrack> removed_stracks;
|
||||
std::vector<STrack> lost_stracks;
|
||||
std::vector<STrack> detections;
|
||||
std::vector<STrack> detections_low;
|
||||
|
||||
std::vector<STrack> detections_cp;
|
||||
std::vector<STrack> tracked_stracks_swap;
|
||||
std::vector<STrack> resa, resb;
|
||||
std::vector<STrack> output_stracks;
|
||||
|
||||
std::vector<STrack *> unconfirmed;
|
||||
std::vector<STrack *> tracked_stracks;
|
||||
std::vector<STrack *> strack_pool;
|
||||
std::vector<STrack *> r_tracked_stracks;
|
||||
|
||||
if (tgts.targets.size() > 0)
|
||||
{
|
||||
for (int i = 0; i < tgts.targets.size(); i++)
|
||||
{
|
||||
|
||||
tgts.targets[i].tracked_id = 0;
|
||||
tgts.targets[i].has_tid = true;
|
||||
|
||||
std::vector<float> tlbr_;
|
||||
tlbr_.resize(4);
|
||||
tlbr_[0] = tgts.targets[i].cx * tgts.width - tgts.targets[i].w * tgts.width / 2;
|
||||
tlbr_[1] = tgts.targets[i].cy * tgts.height - tgts.targets[i].h * tgts.height / 2;
|
||||
tlbr_[2] = tgts.targets[i].cx * tgts.width + tgts.targets[i].w * tgts.width / 2;
|
||||
tlbr_[3] = tgts.targets[i].cy * tgts.height + tgts.targets[i].h * tgts.height / 2;
|
||||
|
||||
float score = tgts.targets[i].score;
|
||||
|
||||
STrack strack(STrack::tlbr_to_tlwh(tlbr_), score);
|
||||
if (score >= track_thresh)
|
||||
{
|
||||
detections.push_back(strack);
|
||||
}
|
||||
else
|
||||
{
|
||||
detections_low.push_back(strack);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Add newly detected tracklets to tracked_stracks
|
||||
for (int i = 0; i < this->tracked_stracks.size(); i++)
|
||||
{
|
||||
if (!this->tracked_stracks[i].is_activated)
|
||||
unconfirmed.push_back(&this->tracked_stracks[i]);
|
||||
else
|
||||
tracked_stracks.push_back(&this->tracked_stracks[i]);
|
||||
}
|
||||
|
||||
////////////////// Step 2: First association, with IoU //////////////////
|
||||
strack_pool = joint_stracks(tracked_stracks, this->lost_stracks);
|
||||
STrack::multi_predict(strack_pool, this->kalman_filter);
|
||||
|
||||
std::vector<std::vector<float>> dists;
|
||||
int dist_size = 0, dist_size_size = 0;
|
||||
dists = iou_distance(strack_pool, detections, dist_size, dist_size_size);
|
||||
|
||||
std::vector<std::vector<int>> matches;
|
||||
std::vector<int> u_track, u_detection;
|
||||
linear_assignment(dists, dist_size, dist_size_size, match_thresh, matches, u_track, u_detection);
|
||||
|
||||
for (int i = 0; i < matches.size(); i++)
|
||||
{
|
||||
STrack *track = strack_pool[matches[i][0]];
|
||||
STrack *det = &detections[matches[i][1]];
|
||||
if (track->state == TrackState::Tracked)
|
||||
{
|
||||
track->update(*det, this->frame_id);
|
||||
activated_stracks.push_back(*track);
|
||||
}
|
||||
else
|
||||
{
|
||||
track->re_activate(*det, this->frame_id, false);
|
||||
refind_stracks.push_back(*track);
|
||||
}
|
||||
}
|
||||
|
||||
////////////////// Step 3: Second association, using low score dets //////////////////
|
||||
for (int i = 0; i < u_detection.size(); i++)
|
||||
{
|
||||
detections_cp.push_back(detections[u_detection[i]]);
|
||||
}
|
||||
detections.clear();
|
||||
detections.assign(detections_low.begin(), detections_low.end());
|
||||
|
||||
for (int i = 0; i < u_track.size(); i++)
|
||||
{
|
||||
if (strack_pool[u_track[i]]->state == TrackState::Tracked)
|
||||
{
|
||||
r_tracked_stracks.push_back(strack_pool[u_track[i]]);
|
||||
}
|
||||
}
|
||||
|
||||
dists.clear();
|
||||
dists = iou_distance(r_tracked_stracks, detections, dist_size, dist_size_size);
|
||||
|
||||
matches.clear();
|
||||
u_track.clear();
|
||||
u_detection.clear();
|
||||
linear_assignment(dists, dist_size, dist_size_size, 0.5, matches, u_track, u_detection);
|
||||
|
||||
for (int i = 0; i < matches.size(); i++)
|
||||
{
|
||||
STrack *track = r_tracked_stracks[matches[i][0]];
|
||||
STrack *det = &detections[matches[i][1]];
|
||||
if (track->state == TrackState::Tracked)
|
||||
{
|
||||
track->update(*det, this->frame_id);
|
||||
activated_stracks.push_back(*track);
|
||||
}
|
||||
else
|
||||
{
|
||||
track->re_activate(*det, this->frame_id, false);
|
||||
refind_stracks.push_back(*track);
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < u_track.size(); i++)
|
||||
{
|
||||
STrack *track = r_tracked_stracks[u_track[i]];
|
||||
if (track->state != TrackState::Lost)
|
||||
{
|
||||
track->mark_lost();
|
||||
lost_stracks.push_back(*track);
|
||||
}
|
||||
}
|
||||
|
||||
// Deal with unconfirmed tracks, usually tracks with only one beginning frame
|
||||
detections.clear();
|
||||
detections.assign(detections_cp.begin(), detections_cp.end());
|
||||
|
||||
dists.clear();
|
||||
dists = iou_distance(unconfirmed, detections, dist_size, dist_size_size);
|
||||
|
||||
matches.clear();
|
||||
std::vector<int> u_unconfirmed;
|
||||
u_detection.clear();
|
||||
linear_assignment(dists, dist_size, dist_size_size, 0.7, matches, u_unconfirmed, u_detection);
|
||||
|
||||
for (int i = 0; i < matches.size(); i++)
|
||||
{
|
||||
unconfirmed[matches[i][0]]->update(detections[matches[i][1]], this->frame_id);
|
||||
activated_stracks.push_back(*unconfirmed[matches[i][0]]);
|
||||
}
|
||||
|
||||
for (int i = 0; i < u_unconfirmed.size(); i++)
|
||||
{
|
||||
STrack *track = unconfirmed[u_unconfirmed[i]];
|
||||
track->mark_removed();
|
||||
removed_stracks.push_back(*track);
|
||||
}
|
||||
|
||||
////////////////// Step 4: Init new stracks //////////////////
|
||||
for (int i = 0; i < u_detection.size(); i++)
|
||||
{
|
||||
STrack *track = &detections[u_detection[i]];
|
||||
if (track->score < this->high_thresh)
|
||||
continue;
|
||||
track->activate(this->kalman_filter, this->frame_id);
|
||||
activated_stracks.push_back(*track);
|
||||
}
|
||||
|
||||
////////////////// Step 5: Update state //////////////////
|
||||
for (int i = 0; i < this->lost_stracks.size(); i++)
|
||||
{
|
||||
if (this->frame_id - this->lost_stracks[i].end_frame() > this->max_time_lost)
|
||||
{
|
||||
this->lost_stracks[i].mark_removed();
|
||||
removed_stracks.push_back(this->lost_stracks[i]);
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < this->tracked_stracks.size(); i++)
|
||||
{
|
||||
if (this->tracked_stracks[i].state == TrackState::Tracked)
|
||||
{
|
||||
tracked_stracks_swap.push_back(this->tracked_stracks[i]);
|
||||
}
|
||||
}
|
||||
this->tracked_stracks.clear();
|
||||
this->tracked_stracks.assign(tracked_stracks_swap.begin(), tracked_stracks_swap.end());
|
||||
|
||||
this->tracked_stracks = joint_stracks(this->tracked_stracks, activated_stracks);
|
||||
this->tracked_stracks = joint_stracks(this->tracked_stracks, refind_stracks);
|
||||
|
||||
// std::cout << activated_stracks.size() << std::endl;
|
||||
|
||||
this->lost_stracks = sub_stracks(this->lost_stracks, this->tracked_stracks);
|
||||
for (int i = 0; i < lost_stracks.size(); i++)
|
||||
{
|
||||
this->lost_stracks.push_back(lost_stracks[i]);
|
||||
}
|
||||
|
||||
this->lost_stracks = sub_stracks(this->lost_stracks, this->removed_stracks);
|
||||
for (int i = 0; i < removed_stracks.size(); i++)
|
||||
{
|
||||
this->removed_stracks.push_back(removed_stracks[i]);
|
||||
}
|
||||
|
||||
remove_duplicate_stracks(resa, resb, this->tracked_stracks, this->lost_stracks);
|
||||
|
||||
this->tracked_stracks.clear();
|
||||
this->tracked_stracks.assign(resa.begin(), resa.end());
|
||||
this->lost_stracks.clear();
|
||||
this->lost_stracks.assign(resb.begin(), resb.end());
|
||||
|
||||
for (int i = 0; i < this->tracked_stracks.size(); i++)
|
||||
{
|
||||
if (this->tracked_stracks[i].is_activated)
|
||||
{
|
||||
output_stracks.push_back(this->tracked_stracks[i]);
|
||||
}
|
||||
}
|
||||
|
||||
tgts.targets.clear();
|
||||
|
||||
for (unsigned long i = 0; i < output_stracks.size(); i++)
|
||||
{
|
||||
std::vector<float> tlwh = output_stracks[i].tlwh;
|
||||
bool vertical = tlwh[2] / tlwh[3] > 1.6;
|
||||
|
||||
if (tlwh[2] * tlwh[3] > 20 && !vertical)
|
||||
{
|
||||
Target tgt;
|
||||
tgt.setBox(tlwh[0], tlwh[1], tlwh[0] + tlwh[2], tlwh[1] + tlwh[3], tgts.width, tgts.height);
|
||||
tgt.setTrackID(output_stracks[i].track_id);
|
||||
tgts.targets.push_back(tgt);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,56 +0,0 @@
|
|||
#ifndef __SV_BYTETrack__
|
||||
#define __SV_BYTETrack__
|
||||
|
||||
#include "sv_core.h"
|
||||
#include "STrack.h"
|
||||
|
||||
class detect_result
|
||||
{
|
||||
public:
|
||||
int classId;
|
||||
float confidence;
|
||||
cv::Rect_<float> box;
|
||||
};
|
||||
|
||||
namespace sv {
|
||||
|
||||
class BYTETracker
|
||||
{
|
||||
public:
|
||||
BYTETracker(int frame_rate = 30, int track_buffer = 30);
|
||||
~BYTETracker();
|
||||
|
||||
void update(TargetsInFrame &tgts);
|
||||
cv::Scalar get_color(int idx);
|
||||
|
||||
private:
|
||||
std::vector<STrack*> joint_stracks( std::vector<STrack*> &tlista, std::vector<STrack> &tlistb);
|
||||
std::vector<STrack> joint_stracks( std::vector<STrack> &tlista, std::vector<STrack> &tlistb);
|
||||
|
||||
std::vector<STrack> sub_stracks( std::vector<STrack> &tlista, std::vector<STrack> &tlistb);
|
||||
void remove_duplicate_stracks( std::vector<STrack> &resa, std::vector<STrack> &resb, std::vector<STrack> &stracksa, std::vector<STrack> &stracksb);
|
||||
|
||||
void linear_assignment( std::vector< std::vector<float> > &cost_matrix, int cost_matrix_size, int cost_matrix_size_size, float thresh,
|
||||
std::vector< std::vector<int> > &matches, std::vector<int> &unmatched_a, std::vector<int> &unmatched_b);
|
||||
std::vector< std::vector<float> > iou_distance( std::vector<STrack*> &atracks, std::vector<STrack> &btracks, int &dist_size, int &dist_size_size);
|
||||
std::vector< std::vector<float> > iou_distance( std::vector<STrack> &atracks, std::vector<STrack> &btracks);
|
||||
std::vector< std::vector<float> > ious( std::vector< std::vector<float> > &atlbrs, std::vector< std::vector<float> > &btlbrs);
|
||||
|
||||
double lapjv(const std::vector< std::vector<float> > &cost, std::vector<int> &rowsol, std::vector<int> &colsol,
|
||||
bool extend_cost = false, float cost_limit = LONG_MAX, bool return_cost = true);
|
||||
|
||||
private:
|
||||
|
||||
float track_thresh;
|
||||
float high_thresh;
|
||||
float match_thresh;
|
||||
int frame_id;
|
||||
int max_time_lost;
|
||||
|
||||
std::vector<STrack> tracked_stracks;
|
||||
std::vector<STrack> lost_stracks;
|
||||
std::vector<STrack> removed_stracks;
|
||||
byte_kalman::ByteKalmanFilter kalman_filter;
|
||||
};
|
||||
}
|
||||
#endif
|
||||
|
|
@ -1,152 +0,0 @@
|
|||
#include "BytekalmanFilter.h"
|
||||
#include <Eigen/Cholesky>
|
||||
|
||||
namespace byte_kalman
|
||||
{
|
||||
const double ByteKalmanFilter::chi2inv95[10] = {
|
||||
0,
|
||||
3.8415,
|
||||
5.9915,
|
||||
7.8147,
|
||||
9.4877,
|
||||
11.070,
|
||||
12.592,
|
||||
14.067,
|
||||
15.507,
|
||||
16.919
|
||||
};
|
||||
ByteKalmanFilter::ByteKalmanFilter()
|
||||
{
|
||||
int ndim = 4;
|
||||
double dt = 1.;
|
||||
|
||||
_motion_mat = Eigen::MatrixXf::Identity(8, 8);
|
||||
for (int i = 0; i < ndim; i++) {
|
||||
_motion_mat(i, ndim + i) = dt;
|
||||
}
|
||||
_update_mat = Eigen::MatrixXf::Identity(4, 8);
|
||||
|
||||
this->_std_weight_position = 1. / 20;
|
||||
this->_std_weight_velocity = 1. / 160;
|
||||
}
|
||||
|
||||
KAL_DATA ByteKalmanFilter::initiate(const DETECTBOX &measurement)
|
||||
{
|
||||
DETECTBOX mean_pos = measurement;
|
||||
DETECTBOX mean_vel;
|
||||
for (int i = 0; i < 4; i++) mean_vel(i) = 0;
|
||||
|
||||
KAL_MEAN mean;
|
||||
for (int i = 0; i < 8; i++) {
|
||||
if (i < 4) mean(i) = mean_pos(i);
|
||||
else mean(i) = mean_vel(i - 4);
|
||||
}
|
||||
|
||||
KAL_MEAN std;
|
||||
std(0) = 2 * _std_weight_position * measurement[3];
|
||||
std(1) = 2 * _std_weight_position * measurement[3];
|
||||
std(2) = 1e-2;
|
||||
std(3) = 2 * _std_weight_position * measurement[3];
|
||||
std(4) = 10 * _std_weight_velocity * measurement[3];
|
||||
std(5) = 10 * _std_weight_velocity * measurement[3];
|
||||
std(6) = 1e-5;
|
||||
std(7) = 10 * _std_weight_velocity * measurement[3];
|
||||
|
||||
KAL_MEAN tmp = std.array().square();
|
||||
KAL_COVA var = tmp.asDiagonal();
|
||||
return std::make_pair(mean, var);
|
||||
}
|
||||
|
||||
void ByteKalmanFilter::predict(KAL_MEAN &mean, KAL_COVA &covariance)
|
||||
{
|
||||
//revise the data;
|
||||
DETECTBOX std_pos;
|
||||
std_pos << _std_weight_position * mean(3),
|
||||
_std_weight_position * mean(3),
|
||||
1e-2,
|
||||
_std_weight_position * mean(3);
|
||||
DETECTBOX std_vel;
|
||||
std_vel << _std_weight_velocity * mean(3),
|
||||
_std_weight_velocity * mean(3),
|
||||
1e-5,
|
||||
_std_weight_velocity * mean(3);
|
||||
KAL_MEAN tmp;
|
||||
tmp.block<1, 4>(0, 0) = std_pos;
|
||||
tmp.block<1, 4>(0, 4) = std_vel;
|
||||
tmp = tmp.array().square();
|
||||
KAL_COVA motion_cov = tmp.asDiagonal();
|
||||
KAL_MEAN mean1 = this->_motion_mat * mean.transpose();
|
||||
KAL_COVA covariance1 = this->_motion_mat * covariance *(_motion_mat.transpose());
|
||||
covariance1 += motion_cov;
|
||||
|
||||
mean = mean1;
|
||||
covariance = covariance1;
|
||||
}
|
||||
|
||||
KAL_HDATA ByteKalmanFilter::project(const KAL_MEAN &mean, const KAL_COVA &covariance)
|
||||
{
|
||||
DETECTBOX std;
|
||||
std << _std_weight_position * mean(3), _std_weight_position * mean(3),
|
||||
1e-1, _std_weight_position * mean(3);
|
||||
KAL_HMEAN mean1 = _update_mat * mean.transpose();
|
||||
KAL_HCOVA covariance1 = _update_mat * covariance * (_update_mat.transpose());
|
||||
Eigen::Matrix<float, 4, 4> diag = std.asDiagonal();
|
||||
diag = diag.array().square().matrix();
|
||||
covariance1 += diag;
|
||||
// covariance1.diagonal() << diag;
|
||||
return std::make_pair(mean1, covariance1);
|
||||
}
|
||||
|
||||
KAL_DATA
|
||||
ByteKalmanFilter::update(
|
||||
const KAL_MEAN &mean,
|
||||
const KAL_COVA &covariance,
|
||||
const DETECTBOX &measurement)
|
||||
{
|
||||
KAL_HDATA pa = project(mean, covariance);
|
||||
KAL_HMEAN projected_mean = pa.first;
|
||||
KAL_HCOVA projected_cov = pa.second;
|
||||
|
||||
//chol_factor, lower =
|
||||
//scipy.linalg.cho_factor(projected_cov, lower=True, check_finite=False)
|
||||
//kalmain_gain =
|
||||
//scipy.linalg.cho_solve((cho_factor, lower),
|
||||
//np.dot(covariance, self._upadte_mat.T).T,
|
||||
//check_finite=False).T
|
||||
Eigen::Matrix<float, 4, 8> B = (covariance * (_update_mat.transpose())).transpose();
|
||||
Eigen::Matrix<float, 8, 4> kalman_gain = (projected_cov.llt().solve(B)).transpose(); // eg.8x4
|
||||
Eigen::Matrix<float, 1, 4> innovation = measurement - projected_mean; //eg.1x4
|
||||
auto tmp = innovation * (kalman_gain.transpose());
|
||||
KAL_MEAN new_mean = (mean.array() + tmp.array()).matrix();
|
||||
KAL_COVA new_covariance = covariance - kalman_gain * projected_cov*(kalman_gain.transpose());
|
||||
return std::make_pair(new_mean, new_covariance);
|
||||
}
|
||||
|
||||
Eigen::Matrix<float, 1, -1>
|
||||
ByteKalmanFilter::gating_distance(
|
||||
const KAL_MEAN &mean,
|
||||
const KAL_COVA &covariance,
|
||||
const std::vector<DETECTBOX> &measurements,
|
||||
bool only_position)
|
||||
{
|
||||
KAL_HDATA pa = this->project(mean, covariance);
|
||||
if (only_position) {
|
||||
printf("not implement!");
|
||||
exit(0);
|
||||
}
|
||||
KAL_HMEAN mean1 = pa.first;
|
||||
KAL_HCOVA covariance1 = pa.second;
|
||||
|
||||
// Eigen::Matrix<float, -1, 4, Eigen::RowMajor> d(size, 4);
|
||||
DETECTBOXSS d(measurements.size(), 4);
|
||||
int pos = 0;
|
||||
for (DETECTBOX box : measurements) {
|
||||
d.row(pos++) = box - mean1;
|
||||
}
|
||||
Eigen::Matrix<float, -1, -1, Eigen::RowMajor> factor = covariance1.llt().matrixL();
|
||||
Eigen::Matrix<float, -1, -1> z = factor.triangularView<Eigen::Lower>().solve<Eigen::OnTheRight>(d).transpose();
|
||||
auto zz = ((z.array())*(z.array())).matrix();
|
||||
auto square_maha = zz.colwise().sum();
|
||||
return square_maha;
|
||||
}
|
||||
}
|
||||
|
|
@ -1,31 +0,0 @@
|
|||
#pragma once
|
||||
|
||||
#include "dataType.h"
|
||||
|
||||
namespace byte_kalman
|
||||
{
|
||||
class ByteKalmanFilter
|
||||
{
|
||||
public:
|
||||
static const double chi2inv95[10];
|
||||
ByteKalmanFilter();
|
||||
KAL_DATA initiate(const DETECTBOX& measurement);
|
||||
void predict(KAL_MEAN& mean, KAL_COVA& covariance);
|
||||
KAL_HDATA project(const KAL_MEAN& mean, const KAL_COVA& covariance);
|
||||
KAL_DATA update(const KAL_MEAN& mean,
|
||||
const KAL_COVA& covariance,
|
||||
const DETECTBOX& measurement);
|
||||
|
||||
Eigen::Matrix<float, 1, -1> gating_distance(
|
||||
const KAL_MEAN& mean,
|
||||
const KAL_COVA& covariance,
|
||||
const std::vector<DETECTBOX>& measurements,
|
||||
bool only_position = false);
|
||||
|
||||
private:
|
||||
Eigen::Matrix<float, 8, 8, Eigen::RowMajor> _motion_mat;
|
||||
Eigen::Matrix<float, 4, 8, Eigen::RowMajor> _update_mat;
|
||||
float _std_weight_position;
|
||||
float _std_weight_velocity;
|
||||
};
|
||||
}
|
||||
|
|
@ -1,192 +0,0 @@
|
|||
#include "STrack.h"
|
||||
|
||||
STrack::STrack( std::vector<float> tlwh_, float score)
|
||||
{
|
||||
_tlwh.resize(4);
|
||||
_tlwh.assign(tlwh_.begin(), tlwh_.end());
|
||||
|
||||
is_activated = false;
|
||||
track_id = 0;
|
||||
state = TrackState::New;
|
||||
|
||||
tlwh.resize(4);
|
||||
tlbr.resize(4);
|
||||
|
||||
static_tlwh();
|
||||
static_tlbr();
|
||||
frame_id = 0;
|
||||
tracklet_len = 0;
|
||||
this->score = score;
|
||||
start_frame = 0;
|
||||
}
|
||||
|
||||
STrack::~STrack()
|
||||
{
|
||||
}
|
||||
|
||||
void STrack::activate(byte_kalman::ByteKalmanFilter &kalman_filter, int frame_id)
|
||||
{
|
||||
this->kalman_filter = kalman_filter;
|
||||
this->track_id = this->next_id();
|
||||
|
||||
std::vector<float> _tlwh_tmp(4);
|
||||
_tlwh_tmp[0] = this->_tlwh[0];
|
||||
_tlwh_tmp[1] = this->_tlwh[1];
|
||||
_tlwh_tmp[2] = this->_tlwh[2];
|
||||
_tlwh_tmp[3] = this->_tlwh[3];
|
||||
std::vector<float> xyah = tlwh_to_xyah(_tlwh_tmp);
|
||||
DETECTBOX xyah_box;
|
||||
xyah_box[0] = xyah[0];
|
||||
xyah_box[1] = xyah[1];
|
||||
xyah_box[2] = xyah[2];
|
||||
xyah_box[3] = xyah[3];
|
||||
auto mc = this->kalman_filter.initiate(xyah_box);
|
||||
this->mean = mc.first;
|
||||
this->covariance = mc.second;
|
||||
|
||||
static_tlwh();
|
||||
static_tlbr();
|
||||
|
||||
this->tracklet_len = 0;
|
||||
this->state = TrackState::Tracked;
|
||||
if (frame_id == 1)
|
||||
{
|
||||
this->is_activated = true;
|
||||
}
|
||||
//this->is_activated = true;
|
||||
this->frame_id = frame_id;
|
||||
this->start_frame = frame_id;
|
||||
}
|
||||
|
||||
void STrack::re_activate(STrack &new_track, int frame_id, bool new_id)
|
||||
{
|
||||
std::vector<float> xyah = tlwh_to_xyah(new_track.tlwh);
|
||||
DETECTBOX xyah_box;
|
||||
xyah_box[0] = xyah[0];
|
||||
xyah_box[1] = xyah[1];
|
||||
xyah_box[2] = xyah[2];
|
||||
xyah_box[3] = xyah[3];
|
||||
auto mc = this->kalman_filter.update(this->mean, this->covariance, xyah_box);
|
||||
this->mean = mc.first;
|
||||
this->covariance = mc.second;
|
||||
|
||||
static_tlwh();
|
||||
static_tlbr();
|
||||
|
||||
this->tracklet_len = 0;
|
||||
this->state = TrackState::Tracked;
|
||||
this->is_activated = true;
|
||||
this->frame_id = frame_id;
|
||||
this->score = new_track.score;
|
||||
if (new_id)
|
||||
this->track_id = next_id();
|
||||
}
|
||||
|
||||
void STrack::update(STrack &new_track, int frame_id)
|
||||
{
|
||||
this->frame_id = frame_id;
|
||||
this->tracklet_len++;
|
||||
|
||||
std::vector<float> xyah = tlwh_to_xyah(new_track.tlwh);
|
||||
DETECTBOX xyah_box;
|
||||
xyah_box[0] = xyah[0];
|
||||
xyah_box[1] = xyah[1];
|
||||
xyah_box[2] = xyah[2];
|
||||
xyah_box[3] = xyah[3];
|
||||
|
||||
auto mc = this->kalman_filter.update(this->mean, this->covariance, xyah_box);
|
||||
this->mean = mc.first;
|
||||
this->covariance = mc.second;
|
||||
|
||||
static_tlwh();
|
||||
static_tlbr();
|
||||
|
||||
this->state = TrackState::Tracked;
|
||||
this->is_activated = true;
|
||||
|
||||
this->score = new_track.score;
|
||||
}
|
||||
|
||||
void STrack::static_tlwh()
|
||||
{
|
||||
if (this->state == TrackState::New)
|
||||
{
|
||||
tlwh[0] = _tlwh[0];
|
||||
tlwh[1] = _tlwh[1];
|
||||
tlwh[2] = _tlwh[2];
|
||||
tlwh[3] = _tlwh[3];
|
||||
return;
|
||||
}
|
||||
|
||||
tlwh[0] = mean[0];
|
||||
tlwh[1] = mean[1];
|
||||
tlwh[2] = mean[2];
|
||||
tlwh[3] = mean[3];
|
||||
|
||||
tlwh[2] *= tlwh[3];
|
||||
tlwh[0] -= tlwh[2] / 2;
|
||||
tlwh[1] -= tlwh[3] / 2;
|
||||
}
|
||||
|
||||
void STrack::static_tlbr()
|
||||
{
|
||||
tlbr.clear();
|
||||
tlbr.assign(tlwh.begin(), tlwh.end());
|
||||
tlbr[2] += tlbr[0];
|
||||
tlbr[3] += tlbr[1];
|
||||
}
|
||||
|
||||
std::vector<float> STrack::tlwh_to_xyah( std::vector<float> tlwh_tmp)
|
||||
{
|
||||
std::vector<float> tlwh_output = tlwh_tmp;
|
||||
tlwh_output[0] += tlwh_output[2] / 2;
|
||||
tlwh_output[1] += tlwh_output[3] / 2;
|
||||
tlwh_output[2] /= tlwh_output[3];
|
||||
return tlwh_output;
|
||||
}
|
||||
|
||||
std::vector<float> STrack::to_xyah()
|
||||
{
|
||||
return tlwh_to_xyah(tlwh);
|
||||
}
|
||||
|
||||
std::vector<float> STrack::tlbr_to_tlwh( std::vector<float> &tlbr)
|
||||
{
|
||||
tlbr[2] -= tlbr[0];
|
||||
tlbr[3] -= tlbr[1];
|
||||
return tlbr;
|
||||
}
|
||||
|
||||
void STrack::mark_lost()
|
||||
{
|
||||
state = TrackState::Lost;
|
||||
}
|
||||
|
||||
void STrack::mark_removed()
|
||||
{
|
||||
state = TrackState::Removed;
|
||||
}
|
||||
|
||||
int STrack::next_id()
|
||||
{
|
||||
static int _count = 0;
|
||||
_count++;
|
||||
return _count;
|
||||
}
|
||||
|
||||
int STrack::end_frame()
|
||||
{
|
||||
return this->frame_id;
|
||||
}
|
||||
|
||||
void STrack::multi_predict( std::vector<STrack*> &stracks, byte_kalman::ByteKalmanFilter &kalman_filter)
|
||||
{
|
||||
for (int i = 0; i < stracks.size(); i++)
|
||||
{
|
||||
if (stracks[i]->state != TrackState::Tracked)
|
||||
{
|
||||
stracks[i]->mean[7] = 0;
|
||||
}
|
||||
kalman_filter.predict(stracks[i]->mean, stracks[i]->covariance);
|
||||
}
|
||||
}
|
||||
|
|
@ -1,47 +0,0 @@
|
|||
#pragma once
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include "BytekalmanFilter.h"
|
||||
|
||||
enum TrackState { New = 0, Tracked, Lost, Removed };
|
||||
|
||||
class STrack
|
||||
{
|
||||
public:
|
||||
STrack( std::vector<float> tlwh_, float score);
|
||||
~STrack();
|
||||
|
||||
std::vector<float> static tlbr_to_tlwh( std::vector<float> &tlbr);
|
||||
void static multi_predict( std::vector<STrack*> &stracks, byte_kalman::ByteKalmanFilter &kalman_filter);
|
||||
void static_tlwh();
|
||||
void static_tlbr();
|
||||
std::vector<float> tlwh_to_xyah( std::vector<float> tlwh_tmp);
|
||||
std::vector<float> to_xyah();
|
||||
void mark_lost();
|
||||
void mark_removed();
|
||||
int next_id();
|
||||
int end_frame();
|
||||
|
||||
void activate(byte_kalman::ByteKalmanFilter &kalman_filter, int frame_id);
|
||||
void re_activate(STrack &new_track, int frame_id, bool new_id = false);
|
||||
void update(STrack &new_track, int frame_id);
|
||||
|
||||
public:
|
||||
bool is_activated;
|
||||
int track_id;
|
||||
int state;
|
||||
|
||||
std::vector<float> _tlwh;
|
||||
std::vector<float> tlwh;
|
||||
std::vector<float> tlbr;
|
||||
int frame_id;
|
||||
int tracklet_len;
|
||||
int start_frame;
|
||||
|
||||
KAL_MEAN mean;
|
||||
KAL_COVA covariance;
|
||||
float score;
|
||||
|
||||
private:
|
||||
byte_kalman::ByteKalmanFilter kalman_filter;
|
||||
};
|
||||
|
|
@ -1,27 +0,0 @@
|
|||
#pragma once
|
||||
|
||||
#include <cstddef>
|
||||
#include <vector>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/Dense>
|
||||
|
||||
|
||||
|
||||
typedef Eigen::Matrix<float, 1, 4, Eigen::RowMajor> DETECTBOX;
|
||||
typedef Eigen::Matrix<float, -1, 4, Eigen::RowMajor> DETECTBOXSS;
|
||||
|
||||
|
||||
//Kalmanfilter
|
||||
typedef Eigen::Matrix<float, 8, 8, Eigen::RowMajor> KAL_FILTER;
|
||||
typedef Eigen::Matrix<float, 1, 8, Eigen::RowMajor> KAL_MEAN;
|
||||
typedef Eigen::Matrix<float, 8, 8, Eigen::RowMajor> KAL_COVA;
|
||||
typedef Eigen::Matrix<float, 1, 4, Eigen::RowMajor> KAL_HMEAN;
|
||||
typedef Eigen::Matrix<float, 4, 4, Eigen::RowMajor> KAL_HCOVA;
|
||||
using KAL_DATA = std::pair<KAL_MEAN, KAL_COVA>;
|
||||
using KAL_HDATA = std::pair<KAL_HMEAN, KAL_HCOVA>;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -1,343 +0,0 @@
|
|||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "lapjv.h"
|
||||
|
||||
/** Column-reduction and reduction transfer for a dense cost matrix.
|
||||
*/
|
||||
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;
|
||||
}
|
||||
|
|
@ -1,63 +0,0 @@
|
|||
#ifndef LAPJV_H
|
||||
#define LAPJV_H
|
||||
|
||||
#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
|
||||
|
||||
|
||||
typedef signed int int_t;
|
||||
typedef unsigned int uint_t;
|
||||
typedef double 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);
|
||||
|
||||
#endif // LAPJV_H
|
||||
|
|
@ -1,432 +0,0 @@
|
|||
#include "BYTETracker.h"
|
||||
#include "lapjv.h"
|
||||
|
||||
namespace sv {
|
||||
|
||||
std::vector<STrack*> BYTETracker::joint_stracks( std::vector<STrack*> &tlista, std::vector<STrack> &tlistb)
|
||||
{
|
||||
std::map<int, int> exists;
|
||||
std::vector<STrack*> res;
|
||||
for (int i = 0; i < tlista.size(); i++)
|
||||
{
|
||||
exists.insert(std::pair<int, int>(tlista[i]->track_id, 1));
|
||||
res.push_back(tlista[i]);
|
||||
}
|
||||
for (int i = 0; i < tlistb.size(); i++)
|
||||
{
|
||||
int tid = tlistb[i].track_id;
|
||||
if (!exists[tid] || exists.count(tid) == 0)
|
||||
{
|
||||
exists[tid] = 1;
|
||||
res.push_back(&tlistb[i]);
|
||||
}
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
std::vector<STrack> BYTETracker::joint_stracks( std::vector<STrack> &tlista, std::vector<STrack> &tlistb)
|
||||
{
|
||||
std::map<int, int> exists;
|
||||
std::vector<STrack> res;
|
||||
for (int i = 0; i < tlista.size(); i++)
|
||||
{
|
||||
exists.insert(std::pair<int, int>(tlista[i].track_id, 1));
|
||||
res.push_back(tlista[i]);
|
||||
}
|
||||
for (int i = 0; i < tlistb.size(); i++)
|
||||
{
|
||||
int tid = tlistb[i].track_id;
|
||||
if (!exists[tid] || exists.count(tid) == 0)
|
||||
{
|
||||
exists[tid] = 1;
|
||||
res.push_back(tlistb[i]);
|
||||
}
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
std::vector<STrack> BYTETracker::sub_stracks( std::vector<STrack> &tlista, std::vector<STrack> &tlistb)
|
||||
{
|
||||
std::map<int, STrack> stracks;
|
||||
for (int i = 0; i < tlista.size(); i++)
|
||||
{
|
||||
stracks.insert(std::pair<int, STrack>(tlista[i].track_id, tlista[i]));
|
||||
}
|
||||
for (int i = 0; i < tlistb.size(); i++)
|
||||
{
|
||||
int tid = tlistb[i].track_id;
|
||||
if (stracks.count(tid) != 0)
|
||||
{
|
||||
stracks.erase(tid);
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<STrack> res;
|
||||
std::map<int, STrack>::iterator it;
|
||||
for (it = stracks.begin(); it != stracks.end(); ++it)
|
||||
{
|
||||
res.push_back(it->second);
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
void BYTETracker::remove_duplicate_stracks( std::vector<STrack> &resa, std::vector<STrack> &resb, std::vector<STrack> &stracksa, std::vector<STrack> &stracksb)
|
||||
{
|
||||
std::vector< std::vector<float> > pdist = iou_distance(stracksa, stracksb);
|
||||
std::vector<std::pair<int, int> > pairs;
|
||||
for (int i = 0; i < pdist.size(); i++)
|
||||
{
|
||||
for (int j = 0; j < pdist[i].size(); j++)
|
||||
{
|
||||
if (pdist[i][j] < 0.15)
|
||||
{
|
||||
pairs.push_back(std::pair<int, int>(i, j));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<int> dupa, dupb;
|
||||
for (int i = 0; i < pairs.size(); i++)
|
||||
{
|
||||
int timep = stracksa[pairs[i].first].frame_id - stracksa[pairs[i].first].start_frame;
|
||||
int timeq = stracksb[pairs[i].second].frame_id - stracksb[pairs[i].second].start_frame;
|
||||
if (timep > timeq)
|
||||
dupb.push_back(pairs[i].second);
|
||||
else
|
||||
dupa.push_back(pairs[i].first);
|
||||
}
|
||||
|
||||
for (int i = 0; i < stracksa.size(); i++)
|
||||
{
|
||||
std::vector<int>::iterator iter = find(dupa.begin(), dupa.end(), i);
|
||||
if (iter == dupa.end())
|
||||
{
|
||||
resa.push_back(stracksa[i]);
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < stracksb.size(); i++)
|
||||
{
|
||||
std::vector<int>::iterator iter = find(dupb.begin(), dupb.end(), i);
|
||||
if (iter == dupb.end())
|
||||
{
|
||||
resb.push_back(stracksb[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void BYTETracker::linear_assignment( std::vector< std::vector<float> > &cost_matrix, int cost_matrix_size, int cost_matrix_size_size, float thresh,
|
||||
std::vector< std::vector<int> > &matches, std::vector<int> &unmatched_a, std::vector<int> &unmatched_b)
|
||||
{
|
||||
if (cost_matrix.size() == 0)
|
||||
{
|
||||
for (int i = 0; i < cost_matrix_size; i++)
|
||||
{
|
||||
unmatched_a.push_back(i);
|
||||
}
|
||||
for (int i = 0; i < cost_matrix_size_size; i++)
|
||||
{
|
||||
unmatched_b.push_back(i);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
std::vector<int> rowsol; std::vector<int> colsol;
|
||||
float c = lapjv(cost_matrix, rowsol, colsol, true, thresh);
|
||||
for (int i = 0; i < rowsol.size(); i++)
|
||||
{
|
||||
if (rowsol[i] >= 0)
|
||||
{
|
||||
std::vector<int> match;
|
||||
match.push_back(i);
|
||||
match.push_back(rowsol[i]);
|
||||
matches.push_back(match);
|
||||
}
|
||||
else
|
||||
{
|
||||
unmatched_a.push_back(i);
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < colsol.size(); i++)
|
||||
{
|
||||
if (colsol[i] < 0)
|
||||
{
|
||||
unmatched_b.push_back(i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::vector< std::vector<float> > BYTETracker::ious( std::vector< std::vector<float> > &atlbrs, std::vector< std::vector<float> > &btlbrs)
|
||||
{
|
||||
std::vector< std::vector<float> > ious;
|
||||
if (atlbrs.size()*btlbrs.size() == 0)
|
||||
return ious;
|
||||
|
||||
ious.resize(atlbrs.size());
|
||||
for (int i = 0; i < ious.size(); i++)
|
||||
{
|
||||
ious[i].resize(btlbrs.size());
|
||||
}
|
||||
|
||||
//bbox_ious
|
||||
for (int k = 0; k < btlbrs.size(); k++)
|
||||
{
|
||||
std::vector<float> ious_tmp;
|
||||
float box_area = (btlbrs[k][2] - btlbrs[k][0] + 1)*(btlbrs[k][3] - btlbrs[k][1] + 1);
|
||||
for (int n = 0; n < atlbrs.size(); n++)
|
||||
{
|
||||
float iw = cv::min(atlbrs[n][2], btlbrs[k][2]) - cv::max(atlbrs[n][0], btlbrs[k][0]) + 1;
|
||||
if (iw > 0)
|
||||
{
|
||||
float ih = cv::min(atlbrs[n][3], btlbrs[k][3]) - cv::max(atlbrs[n][1], btlbrs[k][1]) + 1;
|
||||
if(ih > 0)
|
||||
{
|
||||
float ua = (atlbrs[n][2] - atlbrs[n][0] + 1)*(atlbrs[n][3] - atlbrs[n][1] + 1) + box_area - iw * ih;
|
||||
ious[n][k] = iw * ih / ua;
|
||||
}
|
||||
else
|
||||
{
|
||||
ious[n][k] = 0.0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ious[n][k] = 0.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return ious;
|
||||
}
|
||||
|
||||
std::vector< std::vector<float> > BYTETracker::iou_distance( std::vector<STrack*> &atracks, std::vector<STrack> &btracks, int &dist_size, int &dist_size_size)
|
||||
{
|
||||
std::vector< std::vector<float> > cost_matrix;
|
||||
if (atracks.size() * btracks.size() == 0)
|
||||
{
|
||||
dist_size = atracks.size();
|
||||
dist_size_size = btracks.size();
|
||||
return cost_matrix;
|
||||
}
|
||||
std::vector< std::vector<float> > atlbrs, btlbrs;
|
||||
for (int i = 0; i < atracks.size(); i++)
|
||||
{
|
||||
atlbrs.push_back(atracks[i]->tlbr);
|
||||
}
|
||||
for (int i = 0; i < btracks.size(); i++)
|
||||
{
|
||||
btlbrs.push_back(btracks[i].tlbr);
|
||||
}
|
||||
|
||||
dist_size = atracks.size();
|
||||
dist_size_size = btracks.size();
|
||||
|
||||
std::vector< std::vector<float> > _ious = ious(atlbrs, btlbrs);
|
||||
|
||||
for (int i = 0; i < _ious.size();i++)
|
||||
{
|
||||
std::vector<float> _iou;
|
||||
for (int j = 0; j < _ious[i].size(); j++)
|
||||
{
|
||||
_iou.push_back(1 - _ious[i][j]);
|
||||
}
|
||||
cost_matrix.push_back(_iou);
|
||||
}
|
||||
|
||||
return cost_matrix;
|
||||
}
|
||||
|
||||
std::vector< std::vector<float> > BYTETracker::iou_distance( std::vector<STrack> &atracks, std::vector<STrack> &btracks)
|
||||
{
|
||||
std::vector< std::vector<float> > atlbrs, btlbrs;
|
||||
for (int i = 0; i < atracks.size(); i++)
|
||||
{
|
||||
atlbrs.push_back(atracks[i].tlbr);
|
||||
}
|
||||
for (int i = 0; i < btracks.size(); i++)
|
||||
{
|
||||
btlbrs.push_back(btracks[i].tlbr);
|
||||
}
|
||||
|
||||
std::vector< std::vector<float> > _ious = ious(atlbrs, btlbrs);
|
||||
std::vector< std::vector<float> > cost_matrix;
|
||||
for (int i = 0; i < _ious.size(); i++)
|
||||
{
|
||||
std::vector<float> _iou;
|
||||
for (int j = 0; j < _ious[i].size(); j++)
|
||||
{
|
||||
_iou.push_back(1 - _ious[i][j]);
|
||||
}
|
||||
cost_matrix.push_back(_iou);
|
||||
}
|
||||
|
||||
return cost_matrix;
|
||||
}
|
||||
|
||||
double BYTETracker::lapjv(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)
|
||||
{
|
||||
std::cout << "set extend_cost=True" << std::endl;
|
||||
system("pause");
|
||||
exit(0);
|
||||
}
|
||||
}
|
||||
|
||||
if (extend_cost || cost_limit < LONG_MAX)
|
||||
{
|
||||
n = n_rows + n_cols;
|
||||
cost_c_extended.resize(n);
|
||||
for (int i = 0; i < cost_c_extended.size(); i++)
|
||||
cost_c_extended[i].resize(n);
|
||||
|
||||
if (cost_limit < LONG_MAX)
|
||||
{
|
||||
for (int i = 0; i < cost_c_extended.size(); i++)
|
||||
{
|
||||
for (int j = 0; j < cost_c_extended[i].size(); j++)
|
||||
{
|
||||
cost_c_extended[i][j] = cost_limit / 2.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
float cost_max = -1;
|
||||
for (int i = 0; i < cost_c.size(); i++)
|
||||
{
|
||||
for (int j = 0; j < cost_c[i].size(); j++)
|
||||
{
|
||||
if (cost_c[i][j] > cost_max)
|
||||
cost_max = cost_c[i][j];
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < cost_c_extended.size(); i++)
|
||||
{
|
||||
for (int j = 0; j < cost_c_extended[i].size(); j++)
|
||||
{
|
||||
cost_c_extended[i][j] = cost_max + 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = n_rows; i < cost_c_extended.size(); i++)
|
||||
{
|
||||
for (int 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());
|
||||
}
|
||||
|
||||
double **cost_ptr;
|
||||
cost_ptr = new double *[sizeof(double *) * n];
|
||||
for (int i = 0; i < n; i++)
|
||||
cost_ptr[i] = new double[sizeof(double) * 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[sizeof(int) * n];
|
||||
int *y_c = new int[sizeof(int) * n];
|
||||
|
||||
int ret = lapjv_internal(n, cost_ptr, x_c, y_c);
|
||||
if (ret != 0)
|
||||
{
|
||||
std::cout << "Calculate Wrong!" << std::endl;
|
||||
system("pause");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
double 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 (int i = 0; i < rowsol.size(); i++)
|
||||
{
|
||||
if (rowsol[i] != -1)
|
||||
{
|
||||
//cout << i << "\t" << rowsol[i] << "\t" << cost_ptr[i][rowsol[i]] << endl;
|
||||
opt += cost_ptr[i][rowsol[i]];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (return_cost)
|
||||
{
|
||||
for (int 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;
|
||||
}
|
||||
|
||||
cv::Scalar BYTETracker::get_color(int idx)
|
||||
{
|
||||
idx += 3;
|
||||
return cv::Scalar(37 * idx % 255, 17 * idx % 255, 29 * idx % 255);
|
||||
}
|
||||
}
|
||||
|
|
@ -1,377 +0,0 @@
|
|||
#include "sort.h"
|
||||
#include <cmath>
|
||||
#include <fstream>
|
||||
#include <limits>
|
||||
#include <vector>
|
||||
#include "gason.h"
|
||||
#include "sv_util.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace Eigen;
|
||||
|
||||
namespace sv {
|
||||
|
||||
|
||||
KalmanFilter::KalmanFilter()
|
||||
{
|
||||
this->_chi2inv95 << 3.8415, 5.9915, 7.8147, 9.4877, 11.070, 12.592, 14.067, 15.507, 16.919;
|
||||
this->_F = MatrixXd::Identity(8, 8);
|
||||
for (int i=0; i<4; i++)
|
||||
{
|
||||
this->_F(i,i+4) = 1.; //1
|
||||
}
|
||||
this->_H = MatrixXd::Identity(4, 8);
|
||||
this->_std_weight_position = 1. / 20; //1./20
|
||||
this->_std_weight_vel = 1. / 160; //1./160
|
||||
}
|
||||
|
||||
KalmanFilter::~KalmanFilter()
|
||||
{
|
||||
}
|
||||
|
||||
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::initiate(Vector4d &bbox)
|
||||
{
|
||||
Matrix<double,8,1> mean;
|
||||
Matrix<double,4,1> zero_vector;
|
||||
zero_vector.setZero();
|
||||
mean << bbox(0), bbox(1), (double)bbox(2) / (double)bbox(3), bbox(3), zero_vector;
|
||||
VectorXd stds(8);
|
||||
stds << 2 * this->_std_weight_position * mean(3), 2 * this->_std_weight_position * mean(3), 0.01, 2 * this->_std_weight_position * mean(3), \
|
||||
10 * this->_std_weight_vel * mean(3), 10 * this->_std_weight_vel * mean(3), 1e-5, 10 * this->_std_weight_vel * mean(3);
|
||||
MatrixXd squared = stds.array().square();
|
||||
Matrix<double, 8, 8> covariances;
|
||||
covariances = squared.asDiagonal();
|
||||
return make_pair(mean, covariances);
|
||||
}
|
||||
|
||||
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::update(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> covariances, sv::Box &box)
|
||||
{
|
||||
VectorXd measurement(4);
|
||||
double a = (double)(box.x2-box.x1) / (double)(box.y2-box.y1);
|
||||
measurement << box.x1+(box.x2-box.x1)/2, box.y1+(box.y2-box.y1)/2, a, box.y2-box.y1;
|
||||
pair<Matrix<double, 4, 1>, Matrix<double, 4, 4> > projected = project(mean, covariances);
|
||||
Matrix<double, 4, 1> projected_mean = projected.first;
|
||||
Matrix<double, 4, 4> projected_cov = projected.second;
|
||||
|
||||
Eigen::LLT<Eigen::MatrixXd> chol_factor(projected_cov);
|
||||
MatrixXd Kalman_gain = (chol_factor.solve((covariances * this->_H.transpose()).transpose())).transpose();
|
||||
|
||||
VectorXd innovation = measurement - projected_mean;
|
||||
Matrix<double, 8, 1> new_mean = mean + Kalman_gain *innovation;
|
||||
Matrix<double, 8, 8> new_covariances = covariances - Kalman_gain * projected_cov * Kalman_gain.transpose();
|
||||
|
||||
return make_pair(new_mean, new_covariances);
|
||||
}
|
||||
|
||||
pair<Matrix<double, 4, 1>, Matrix<double, 4, 4> > KalmanFilter::project(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> covariances)
|
||||
{
|
||||
VectorXd stds(4);
|
||||
stds << this->_std_weight_position * mean(3), this->_std_weight_position * mean(3), 0.1, this->_std_weight_position * mean(3);
|
||||
MatrixXd squared = stds.array().square();
|
||||
MatrixXd R = squared.asDiagonal();
|
||||
|
||||
Matrix<double, 4, 1> pro_mean = this->_H * mean;
|
||||
Matrix<double, 4, 4> pro_covariances = this->_H * covariances * this->_H.transpose() + R;
|
||||
return make_pair(pro_mean, pro_covariances);
|
||||
}
|
||||
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::predict(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> covariances)
|
||||
{
|
||||
VectorXd stds(8);
|
||||
stds << this->_std_weight_position * mean(3), this->_std_weight_position * mean(3), 1e-2, this->_std_weight_position * mean(3), \
|
||||
this->_std_weight_vel * mean(3), this->_std_weight_vel * mean(3), 1e-5, this->_std_weight_vel * mean(3); // a = 0.01
|
||||
MatrixXd squared = stds.array().square();
|
||||
MatrixXd Q = squared.asDiagonal();
|
||||
Matrix<double, 8, 1> pre_mean = this->_F * mean;
|
||||
Matrix<double, 8, 8> pre_cov = this->_F * covariances * this->_F.transpose()+Q;//+Q
|
||||
return make_pair(pre_mean, pre_cov);
|
||||
}
|
||||
|
||||
|
||||
SORT::~SORT()
|
||||
{
|
||||
}
|
||||
|
||||
void SORT::update(TargetsInFrame& tgts)
|
||||
{
|
||||
KalmanFilter kf;
|
||||
if (! this->_tracklets.size() || tgts.targets.size() == 0)
|
||||
{
|
||||
Vector4d bbox;
|
||||
for (int i=0; i<tgts.targets.size(); i++)
|
||||
{
|
||||
sv::Box box;
|
||||
tgts.targets[i].getBox(box);
|
||||
Tracklet tracklet;
|
||||
tracklet.id = ++ this->_next_tracklet_id;
|
||||
tgts.targets[i].tracked_id = this->_next_tracklet_id;
|
||||
tgts.targets[i].has_tid = true;
|
||||
|
||||
tracklet.bbox << box.x1+(box.x2-box.x1)/2, box.y1+(box.y2-box.y1)/2, box.x2-box.x1, box.y2-box.y1; // x,y,w,h; center(x,y)
|
||||
|
||||
tracklet.age = 0;
|
||||
tracklet.hits = 1;
|
||||
//tracklet.misses = 0;
|
||||
tracklet.frame_id = tgts.frame_id;
|
||||
tracklet.category_id = tgts.targets[i].category_id;
|
||||
if (tgts.frame_id == 0)
|
||||
{
|
||||
tracklet.tentative = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
tracklet.tentative = true;
|
||||
}
|
||||
// initate the motion
|
||||
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > motion = kf.initiate(tracklet.bbox);
|
||||
tracklet.mean = motion.first;
|
||||
tracklet.covariance = motion.second;
|
||||
|
||||
this->_tracklets.push_back(tracklet);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int i=0; i<tgts.targets.size(); i++)
|
||||
{
|
||||
tgts.targets[i].tracked_id = 0;
|
||||
tgts.targets[i].has_tid = true;
|
||||
}
|
||||
|
||||
vector<int> match_det(tgts.targets.size(), -1);
|
||||
// predict the next state of each tracklet
|
||||
for (auto& tracklet : this->_tracklets)
|
||||
{
|
||||
tracklet.age++;
|
||||
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > motion = kf.predict(tracklet.mean, tracklet.covariance);
|
||||
tracklet.bbox << motion.first(0), motion.first(1), motion.first(2) * motion.first(3), motion.first(3);
|
||||
tracklet.mean = motion.first;
|
||||
tracklet.covariance = motion.second;
|
||||
}
|
||||
|
||||
// Match the detections to the existing tracklets
|
||||
vector<vector<double> > iouMatrix(this->_tracklets.size(), vector<double> (tgts.targets.size(), 0));
|
||||
for (int i=0; i<this->_tracklets.size(); i++)
|
||||
{
|
||||
for (int j=0; j<tgts.targets.size(); j++)
|
||||
{
|
||||
sv::Box box;
|
||||
tgts.targets[j].getBox(box);
|
||||
iouMatrix[i][j] = this->_iou(this->_tracklets[i], box);
|
||||
}
|
||||
}
|
||||
|
||||
vector<pair<int, int> > matches = this->_hungarian(iouMatrix);
|
||||
for (auto& match : matches)
|
||||
{
|
||||
int trackletIndex = match.first;
|
||||
int detectionIndex = match.second;
|
||||
if (trackletIndex >= 0 && detectionIndex >= 0)
|
||||
{
|
||||
if (iouMatrix[match.first][match.second] <= 1-_iou_threshold) // iou_thrshold
|
||||
{
|
||||
sv::Box box;
|
||||
tgts.targets[detectionIndex].getBox(box);
|
||||
this->_tracklets[trackletIndex].age = 0;
|
||||
this->_tracklets[trackletIndex].hits++;
|
||||
this->_tracklets[trackletIndex].frame_id = tgts.frame_id;
|
||||
this->_tracklets[trackletIndex].bbox << box.x1+(box.x2-box.x1)/2, box.y1+(box.y2-box.y1)/2, box.x2-box.x1, box.y2-box.y1;
|
||||
tgts.targets[detectionIndex].tracked_id = this->_tracklets[trackletIndex].id;
|
||||
match_det[detectionIndex] = trackletIndex;
|
||||
}
|
||||
}
|
||||
}
|
||||
std::vector <vector<double>> ().swap(iouMatrix);
|
||||
for (int i=0; i<tgts.targets.size(); i++)
|
||||
{
|
||||
if (match_det[i] == -1)
|
||||
{
|
||||
sv::Box box;
|
||||
tgts.targets[i].getBox(box);
|
||||
Tracklet tracklet;
|
||||
tracklet.id = ++ this->_next_tracklet_id;
|
||||
tracklet.bbox << box.x1+(box.x2-box.x1)/2, (double)(box.y1+(box.y2-box.y1)/2), box.x2-box.x1, box.y2-box.y1; // c_x, c_y, w, h
|
||||
tracklet.age = 0;
|
||||
tracklet.hits = 1;
|
||||
tracklet.frame_id = tgts.frame_id;
|
||||
tracklet.category_id = tgts.targets[i].category_id;
|
||||
tracklet.tentative = true;
|
||||
|
||||
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > new_motion = kf.initiate(tracklet.bbox);
|
||||
tracklet.mean = new_motion.first;
|
||||
tracklet.covariance = new_motion.second;
|
||||
|
||||
tgts.targets[i].tracked_id = this->_next_tracklet_id;
|
||||
tgts.targets[i].has_tid = true;
|
||||
this->_tracklets.push_back(tracklet);
|
||||
}
|
||||
else
|
||||
{
|
||||
sv::Box box;
|
||||
int track_id = match_det[i];
|
||||
tgts.targets[i].getBox(box);
|
||||
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > updated = kf.update(this->_tracklets[track_id].mean, this->_tracklets[track_id].covariance, box);
|
||||
this->_tracklets[track_id].mean = updated.first;
|
||||
this->_tracklets[track_id].covariance = updated.second;
|
||||
}
|
||||
}
|
||||
|
||||
//sift tracklets
|
||||
for (auto& tracklet : this->_tracklets)
|
||||
{
|
||||
if (tracklet.hits >= _min_hits)
|
||||
{
|
||||
tracklet.tentative = false;
|
||||
}
|
||||
if ((tgts.frame_id-tracklet.frame_id <= _max_age) && !(tracklet.tentative && tracklet.frame_id != tgts.frame_id))
|
||||
{
|
||||
_new_tracklets.push_back(tracklet);
|
||||
}
|
||||
}
|
||||
_tracklets = _new_tracklets;
|
||||
std::vector <Tracklet> ().swap(_new_tracklets);
|
||||
}
|
||||
}
|
||||
|
||||
vector<Tracklet> SORT::getTracklets() const
|
||||
{
|
||||
return this->_tracklets;
|
||||
}
|
||||
|
||||
double SORT::_iou(Tracklet& tracklet, sv::Box& box)
|
||||
{
|
||||
double trackletX1 = tracklet.bbox(0)-tracklet.bbox(2)/2;
|
||||
double trackletY1 = tracklet.bbox(1)-tracklet.bbox(3)/2;
|
||||
double trackletX2 = tracklet.bbox(0) + tracklet.bbox(2)/2;
|
||||
double trackletY2 = tracklet.bbox(1) + tracklet.bbox(3)/2;
|
||||
|
||||
double detectionX1 = box.x1;
|
||||
double detectionY1 = box.y1;
|
||||
double detectionX2 = box.x2;
|
||||
double detectionY2 = box.y2;
|
||||
|
||||
double intersectionX1 = max(trackletX1, detectionX1);
|
||||
double intersectionY1 = max(trackletY1, detectionY1);
|
||||
double intersectionX2 = min(trackletX2, detectionX2);
|
||||
double intersectionY2 = min(trackletY2, detectionY2);
|
||||
|
||||
double w = (intersectionX2-intersectionX1 > 0.0) ? (intersectionX2-intersectionX1) : 0.0;
|
||||
double h = (intersectionY2-intersectionY1 > 0.0) ? (intersectionY2-intersectionY1) : 0.0;
|
||||
double intersectionArea = w * h;
|
||||
|
||||
double trackletArea = tracklet.bbox(2) * tracklet.bbox(3);
|
||||
|
||||
double detectionArea = (box.x2-box.x1) * (box.y2-box.y1);
|
||||
double unionArea = trackletArea + detectionArea - intersectionArea;
|
||||
double iou = (1 - intersectionArea / unionArea * 1.0);
|
||||
|
||||
return iou;
|
||||
}
|
||||
|
||||
// Function to find the minimum element in a vector
|
||||
double SORT::_findMin(const std::vector<double>& vec) {
|
||||
double minVal = std::numeric_limits<double>::max();
|
||||
for (double val : vec) {
|
||||
if (val < minVal) {
|
||||
minVal = val;
|
||||
}
|
||||
}
|
||||
return minVal;
|
||||
}
|
||||
|
||||
// Function to subtract the minimum value from each row of the cost matrix
|
||||
void SORT::_subtractMinFromRows(std::vector<std::vector<double>>& costMatrix) {
|
||||
for (auto& row : costMatrix) {
|
||||
double minVal = _findMin(row);
|
||||
for (double& val : row) {
|
||||
val -= minVal;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Function to subtract the minimum value from each column of the cost matrix
|
||||
void SORT::_subtractMinFromCols(std::vector<std::vector<double>>& costMatrix) {
|
||||
for (size_t col = 0; col < costMatrix[0].size(); ++col) {
|
||||
double minVal = std::numeric_limits<double>::max();
|
||||
for (size_t row = 0; row < costMatrix.size(); ++row) {
|
||||
if (costMatrix[row][col] < minVal) {
|
||||
minVal = costMatrix[row][col];
|
||||
}
|
||||
}
|
||||
for (size_t row = 0; row < costMatrix.size(); ++row) {
|
||||
costMatrix[row][col] -= minVal;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Function to find a matching using the Hungarian algorithm
|
||||
vector<pair<int, int> > SORT::_hungarian(vector<vector<double> > costMatrix)
|
||||
{
|
||||
size_t numRows = costMatrix.size();
|
||||
size_t numCols = costMatrix[0].size();
|
||||
|
||||
//transpose the matrix if necessary
|
||||
const bool transposed = numCols > numRows;
|
||||
if (transposed) {
|
||||
vector<vector<double>> transposedMatrix(numCols, vector<double>(numRows));
|
||||
for (int i = 0; i < numRows; i++)
|
||||
{
|
||||
for (int j = 0; j < numCols; j++)
|
||||
{
|
||||
transposedMatrix[j][i] = costMatrix[i][j];
|
||||
}
|
||||
}
|
||||
costMatrix = transposedMatrix;
|
||||
swap(numRows, numCols);
|
||||
}
|
||||
// Determine the larger dimension for matching
|
||||
size_t maxDim = std::max(numRows, numCols);
|
||||
|
||||
// Create a square cost matrix by padding with zeros if necessary
|
||||
std::vector<std::vector<double>> squareMatrix(maxDim, std::vector<double>(maxDim, 0.0));
|
||||
for (size_t row = 0; row < numRows; ++row) {
|
||||
for (size_t col = 0; col < numCols; ++col) {
|
||||
squareMatrix[row][col] = costMatrix[row][col];
|
||||
}
|
||||
}
|
||||
|
||||
// Subtract the minimum value from each row and column
|
||||
_subtractMinFromRows(squareMatrix);
|
||||
_subtractMinFromCols(squareMatrix);
|
||||
|
||||
// Initialize the assignment vectors with -1 values
|
||||
std::vector<int> rowAssignment(maxDim, -1);
|
||||
std::vector<int> colAssignment(maxDim, -1);
|
||||
|
||||
// Perform the matching
|
||||
for (size_t row = 0; row < maxDim; ++row) {
|
||||
std::vector<bool> visitedCols(maxDim, false);
|
||||
for (size_t col = 0; col < maxDim; ++col) {
|
||||
if (squareMatrix[row][col] == 0 && colAssignment[col] == -1) {
|
||||
rowAssignment[row] = col;
|
||||
colAssignment[col] = row;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Convert the assignment vectors to pair<int, int> format
|
||||
std::vector<std::pair<int, int>> assignmentPairs;
|
||||
for (size_t row = 0; row < numRows; ++row) {
|
||||
int col = rowAssignment[row];
|
||||
if (col != -1) {
|
||||
if (col >= numCols) {
|
||||
col = -1;
|
||||
}
|
||||
assignmentPairs.emplace_back(row, col);
|
||||
}
|
||||
}
|
||||
if (transposed) {
|
||||
for (auto& assignment : assignmentPairs)
|
||||
{
|
||||
swap(assignment.first, assignment.second);
|
||||
}
|
||||
}
|
||||
return assignmentPairs;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1,77 +0,0 @@
|
|||
#ifndef __SV_SORT__
|
||||
#define __SV_SORT__
|
||||
|
||||
#include "sv_core.h"
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <opencv2/aruco.hpp>
|
||||
#include <opencv2/tracking.hpp>
|
||||
#include <string>
|
||||
#include <chrono>
|
||||
#include <Eigen/Dense>
|
||||
|
||||
|
||||
namespace sv {
|
||||
|
||||
|
||||
|
||||
// define the tracklet struct to store the tracked objects.
|
||||
struct Tracklet
|
||||
{
|
||||
/* data */
|
||||
public:
|
||||
Eigen::Vector4d bbox; // double x, y, w, h;
|
||||
int id = 0;
|
||||
int age;
|
||||
int hits;
|
||||
int misses;
|
||||
int frame_id = 0;
|
||||
int category_id;
|
||||
bool tentative;
|
||||
std::vector<double> features;
|
||||
Eigen::Matrix<double, 8, 1> mean;
|
||||
Eigen::Matrix<double, 8, 8> covariance;
|
||||
};
|
||||
|
||||
|
||||
class KalmanFilter {
|
||||
public:
|
||||
KalmanFilter();
|
||||
~KalmanFilter();
|
||||
std::pair<Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8> > initiate(Eigen::Vector4d &bbox);
|
||||
std::pair<Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8> > update(Eigen::Matrix<double, 8, 1> mean, Eigen::Matrix<double, 8, 8> covariances, Box &box);
|
||||
std::pair<Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8> > predict(Eigen::Matrix<double, 8, 1> mean, Eigen::Matrix<double, 8, 8> covariances);
|
||||
std::pair<Eigen::Matrix<double, 4, 1>, Eigen::Matrix<double, 4, 4> > project(Eigen::Matrix<double, 8, 1> mean, Eigen::Matrix<double, 8, 8> covariances);
|
||||
private:
|
||||
Eigen::Matrix<double, 8, 8> _F;
|
||||
Eigen::Matrix<double, 4, 8> _H;
|
||||
Eigen::Matrix<double, 9, 1> _chi2inv95;
|
||||
double _std_weight_position;
|
||||
double _std_weight_vel;
|
||||
};
|
||||
|
||||
|
||||
class SORT {
|
||||
public:
|
||||
SORT(double iou_threshold, int max_age, int min_hits): _iou_threshold(iou_threshold), _max_age(max_age), _min_hits(min_hits), _next_tracklet_id(0) {};
|
||||
~SORT();
|
||||
void update(TargetsInFrame &tgts);
|
||||
std::vector<Tracklet> getTracklets() const;
|
||||
private:
|
||||
double _iou(Tracklet &tracklet, Box &box);
|
||||
std::vector<std::pair<int,int>> _hungarian(std::vector<std::vector<double>> costMatrix);
|
||||
double _findMin(const std::vector<double>& vec);
|
||||
void _subtractMinFromRows(std::vector<std::vector<double>>& costMatrix);
|
||||
void _subtractMinFromCols(std::vector<std::vector<double>>& costMatrix);
|
||||
//bool _augment(const std::vector<std::vector<double>>& costMatrix, int row, std::vector<int>& rowMatch, std::vector<int>& colMatch, std::vector<bool>& visited);
|
||||
|
||||
double _iou_threshold;
|
||||
int _max_age;
|
||||
int _min_hits;
|
||||
int _next_tracklet_id;
|
||||
std::vector <Tracklet> _tracklets;
|
||||
std::vector <Tracklet> _new_tracklets;
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
#endif
|
||||
|
|
@ -5,8 +5,6 @@
|
|||
#include <vector>
|
||||
#include "gason.h"
|
||||
#include "sv_util.h"
|
||||
#include "sort.h"
|
||||
#include "BYTETracker.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace Eigen;
|
||||
|
|
@ -18,22 +16,15 @@ MultipleObjectTracker::MultipleObjectTracker()
|
|||
{
|
||||
this->_params_loaded = false;
|
||||
this->_sort_impl = NULL;
|
||||
this->_bytetrack_impl = NULL;
|
||||
}
|
||||
MultipleObjectTracker::~MultipleObjectTracker()
|
||||
{
|
||||
if (this->_sort_impl)
|
||||
delete this->_sort_impl;
|
||||
else if (this->_bytetrack_impl)
|
||||
delete this->_bytetrack_impl;
|
||||
}
|
||||
|
||||
sv::TargetsInFrame MultipleObjectTracker::track(cv::Mat img_, TargetsInFrame& tgts_)
|
||||
void MultipleObjectTracker::track(cv::Mat img_, TargetsInFrame& tgts_)
|
||||
{
|
||||
sv::TargetsInFrame person_tgts(tgts_.frame_id);
|
||||
person_tgts.width = img_.size().width;
|
||||
person_tgts.height = img_.size().height;
|
||||
|
||||
if (!this->_params_loaded)
|
||||
{
|
||||
this->_load();
|
||||
|
|
@ -42,28 +33,8 @@ sv::TargetsInFrame MultipleObjectTracker::track(cv::Mat img_, TargetsInFrame& tg
|
|||
if ("sort" == this->_algorithm && this->_sort_impl)
|
||||
{
|
||||
this->_detector->detect(img_, tgts_);
|
||||
for (auto target : tgts_.targets)
|
||||
{
|
||||
if (target.category_id == 0)
|
||||
{
|
||||
person_tgts.targets.push_back(target);
|
||||
}
|
||||
}
|
||||
this->_sort_impl->update(person_tgts);
|
||||
this->_sort_impl->update(tgts_);
|
||||
}
|
||||
else if ("bytetrack" == this->_algorithm && this->_bytetrack_impl)
|
||||
{
|
||||
this->_detector->detect(img_, tgts_);
|
||||
for (auto target : tgts_.targets)
|
||||
{
|
||||
if (target.category_id == 0)
|
||||
{
|
||||
person_tgts.targets.push_back(target);
|
||||
}
|
||||
}
|
||||
this->_bytetrack_impl->update(person_tgts);
|
||||
}
|
||||
return person_tgts;
|
||||
}
|
||||
|
||||
void MultipleObjectTracker::init(CommonObjectDetector* detector_)
|
||||
|
|
@ -77,10 +48,6 @@ void MultipleObjectTracker::init(CommonObjectDetector* detector_)
|
|||
{
|
||||
this->_sort_impl = new SORT(this->_iou_thres, this->_max_age, this->_min_hits);
|
||||
}
|
||||
else if("bytetrack" == this->_algorithm)
|
||||
{
|
||||
this->_bytetrack_impl = new BYTETracker(this->_frame_rate, this->_track_buffer);
|
||||
}
|
||||
this->_detector = detector_;
|
||||
}
|
||||
|
||||
|
|
@ -104,7 +71,7 @@ void MultipleObjectTracker::_load()
|
|||
else
|
||||
this->_with_reid = false;
|
||||
|
||||
//std::cout << "with_reid: " << this->_with_reid << std::endl;
|
||||
std::cout << "with_reid: " << this->_with_reid << std::endl;
|
||||
}
|
||||
else if ("reid_input_size" == std::string(i->key) && i->value.getTag() == JSON_ARRAY) {
|
||||
int jcnt = 0;
|
||||
|
|
@ -117,39 +84,395 @@ void MultipleObjectTracker::_load()
|
|||
}
|
||||
jcnt += 1;
|
||||
}
|
||||
//std::cout << "reid_input_w: " << this->_reid_input_w << std::endl;
|
||||
//std::cout << "reid_input_h: " << this->_reid_input_h << std::endl;
|
||||
std::cout << "reid_input_w: " << this->_reid_input_w << std::endl;
|
||||
std::cout << "reid_input_h: " << this->_reid_input_h << std::endl;
|
||||
}
|
||||
else if ("reid_num_samples" == std::string(i->key)) {
|
||||
this->_reid_num_samples = i->value.toNumber();
|
||||
//std::cout << "reid_num_samples: " << this->_reid_num_samples << std::endl;
|
||||
std::cout << "reid_num_samples: " << this->_reid_num_samples << std::endl;
|
||||
}
|
||||
else if ("reid_match_thres" == std::string(i->key)) {
|
||||
this->_reid_match_thres = i->value.toNumber();
|
||||
//std::cout << "reid_match_thres: " << this->_reid_match_thres << std::endl;
|
||||
std::cout << "reid_match_thres: " << this->_reid_match_thres << std::endl;
|
||||
}
|
||||
else if ("iou_thres" == std::string(i->key)) {
|
||||
this->_iou_thres = i->value.toNumber();
|
||||
//std::cout << "iou_thres: " << this->_iou_thres << std::endl;
|
||||
std::cout << "iou_thres: " << this->_iou_thres << std::endl;
|
||||
}
|
||||
else if ("max_age" == std::string(i->key)) {
|
||||
this->_max_age = i->value.toNumber();
|
||||
//std::cout << "max_age: " << this->_max_age << std::endl;
|
||||
std::cout << "max_age: " << this->_max_age << std::endl;
|
||||
}
|
||||
else if ("min_hits" == std::string(i->key)) {
|
||||
this->_min_hits = i->value.toNumber();
|
||||
//std::cout << "min_hits: " << this->_min_hits << std::endl;
|
||||
}
|
||||
else if ("frame_rate" == std::string(i->key)) {
|
||||
this->_frame_rate = i->value.toNumber();
|
||||
//std::cout << "max_age: " << this->_max_age << std::endl;
|
||||
}
|
||||
else if ("track_buffer" == std::string(i->key)) {
|
||||
this->_track_buffer = i->value.toNumber();
|
||||
//std::cout << "min_hits: " << this->_min_hits << std::endl;
|
||||
std::cout << "min_hits: " << this->_min_hits << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
KalmanFilter::KalmanFilter()
|
||||
{
|
||||
this->_chi2inv95 << 3.8415, 5.9915, 7.8147, 9.4877, 11.070, 12.592, 14.067, 15.507, 16.919;
|
||||
this->_F = MatrixXd::Identity(8, 8);
|
||||
for (int i=0; i<4; i++)
|
||||
{
|
||||
this->_F(i,i+4) = 1.; //1
|
||||
}
|
||||
this->_H = MatrixXd::Identity(4, 8);
|
||||
this->_std_weight_position = 1. / 20; //1./20
|
||||
this->_std_weight_vel = 1. / 160; //1./160
|
||||
}
|
||||
|
||||
KalmanFilter::~KalmanFilter()
|
||||
{
|
||||
}
|
||||
|
||||
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::initiate(Vector4d &bbox)
|
||||
{
|
||||
Matrix<double,8,1> mean;
|
||||
Matrix<double,4,1> zero_vector;
|
||||
zero_vector.setZero();
|
||||
mean << bbox(0), bbox(1), (double)bbox(2) / (double)bbox(3), bbox(3), zero_vector;
|
||||
VectorXd stds(8);
|
||||
stds << 2 * this->_std_weight_position * mean(3), 2 * this->_std_weight_position * mean(3), 0.01, 2 * this->_std_weight_position * mean(3), \
|
||||
10 * this->_std_weight_vel * mean(3), 10 * this->_std_weight_vel * mean(3), 1e-5, 10 * this->_std_weight_vel * mean(3);
|
||||
MatrixXd squared = stds.array().square();
|
||||
Matrix<double, 8, 8> covariances;
|
||||
covariances = squared.asDiagonal();
|
||||
return make_pair(mean, covariances);
|
||||
}
|
||||
|
||||
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::update(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> covariances, sv::Box &box)
|
||||
{
|
||||
VectorXd measurement(4);
|
||||
double a = (double)(box.x2-box.x1) / (double)(box.y2-box.y1);
|
||||
measurement << box.x1+(box.x2-box.x1)/2, box.y1+(box.y2-box.y1)/2, a, box.y2-box.y1;
|
||||
pair<Matrix<double, 4, 1>, Matrix<double, 4, 4> > projected = project(mean, covariances);
|
||||
Matrix<double, 4, 1> projected_mean = projected.first;
|
||||
Matrix<double, 4, 4> projected_cov = projected.second;
|
||||
|
||||
Eigen::LLT<Eigen::MatrixXd> chol_factor(projected_cov);
|
||||
MatrixXd Kalman_gain = (chol_factor.solve((covariances * this->_H.transpose()).transpose())).transpose();
|
||||
|
||||
VectorXd innovation = measurement - projected_mean;
|
||||
Matrix<double, 8, 1> new_mean = mean + Kalman_gain *innovation;
|
||||
Matrix<double, 8, 8> new_covariances = covariances - Kalman_gain * projected_cov * Kalman_gain.transpose();
|
||||
|
||||
return make_pair(new_mean, new_covariances);
|
||||
}
|
||||
|
||||
pair<Matrix<double, 4, 1>, Matrix<double, 4, 4> > KalmanFilter::project(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> covariances)
|
||||
{
|
||||
VectorXd stds(4);
|
||||
stds << this->_std_weight_position * mean(3), this->_std_weight_position * mean(3), 0.1, this->_std_weight_position * mean(3);
|
||||
MatrixXd squared = stds.array().square();
|
||||
MatrixXd R = squared.asDiagonal();
|
||||
|
||||
Matrix<double, 4, 1> pro_mean = this->_H * mean;
|
||||
Matrix<double, 4, 4> pro_covariances = this->_H * covariances * this->_H.transpose() + R;
|
||||
return make_pair(pro_mean, pro_covariances);
|
||||
}
|
||||
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > KalmanFilter::predict(Matrix<double, 8, 1> mean, Matrix<double, 8, 8> covariances)
|
||||
{
|
||||
VectorXd stds(8);
|
||||
stds << this->_std_weight_position * mean(3), this->_std_weight_position * mean(3), 1e-2, this->_std_weight_position * mean(3), \
|
||||
this->_std_weight_vel * mean(3), this->_std_weight_vel * mean(3), 1e-5, this->_std_weight_vel * mean(3); // a = 0.01
|
||||
MatrixXd squared = stds.array().square();
|
||||
MatrixXd Q = squared.asDiagonal();
|
||||
Matrix<double, 8, 1> pre_mean = this->_F * mean;
|
||||
Matrix<double, 8, 8> pre_cov = this->_F * covariances * this->_F.transpose()+Q;//+Q
|
||||
return make_pair(pre_mean, pre_cov);
|
||||
}
|
||||
|
||||
|
||||
SORT::~SORT()
|
||||
{
|
||||
}
|
||||
|
||||
void SORT::update(TargetsInFrame& tgts)
|
||||
{
|
||||
sv::KalmanFilter kf;
|
||||
if (! this->_tracklets.size() || tgts.targets.size() == 0)
|
||||
{
|
||||
Vector4d bbox;
|
||||
for (int i=0; i<tgts.targets.size(); i++)
|
||||
{
|
||||
sv::Box box;
|
||||
tgts.targets[i].getBox(box);
|
||||
Tracklet tracklet;
|
||||
tracklet.id = ++ this->_next_tracklet_id;
|
||||
tgts.targets[i].tracked_id = this->_next_tracklet_id;
|
||||
tgts.targets[i].has_tid = true;
|
||||
|
||||
tracklet.bbox << box.x1+(box.x2-box.x1)/2, box.y1+(box.y2-box.y1)/2, box.x2-box.x1, box.y2-box.y1; // x,y,w,h; center(x,y)
|
||||
|
||||
tracklet.age = 0;
|
||||
tracklet.hits = 1;
|
||||
tracklet.misses = 0;
|
||||
tracklet.frame_id = tgts.frame_id;
|
||||
tracklet.category_id = tgts.targets[i].category_id;
|
||||
tracklet.tentative = true;
|
||||
|
||||
// initate the motion
|
||||
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > motion = kf.initiate(tracklet.bbox);
|
||||
tracklet.mean = motion.first;
|
||||
tracklet.covariance = motion.second;
|
||||
|
||||
this->_tracklets.push_back(tracklet);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// cout << "frame id:" << tgts.frame_id << endl;
|
||||
for (int i=0; i<tgts.targets.size(); i++)
|
||||
{
|
||||
tgts.targets[i].tracked_id = 0;
|
||||
tgts.targets[i].has_tid = true;
|
||||
}
|
||||
|
||||
vector<int> match_det(tgts.targets.size(), -1);
|
||||
// predict the next state of each tracklet
|
||||
for (auto& tracklet : this->_tracklets)
|
||||
{
|
||||
tracklet.age++;
|
||||
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > motion = kf.predict(tracklet.mean, tracklet.covariance);
|
||||
tracklet.bbox << motion.first(0), motion.first(1), motion.first(2) * motion.first(3), motion.first(3);
|
||||
tracklet.mean = motion.first;
|
||||
tracklet.covariance = motion.second;
|
||||
}
|
||||
|
||||
// Match the detections to the existing tracklets
|
||||
vector<vector<double> > iouMatrix(this->_tracklets.size(), vector<double> (tgts.targets.size(), 0));
|
||||
for (int i=0; i<this->_tracklets.size(); i++)
|
||||
{
|
||||
for (int j=0; j<tgts.targets.size(); j++)
|
||||
{
|
||||
sv::Box box;
|
||||
tgts.targets[j].getBox(box);
|
||||
iouMatrix[i][j] = this->_iou(this->_tracklets[i], box);
|
||||
}
|
||||
}
|
||||
|
||||
vector<pair<int, int> > matches = this->_hungarian(iouMatrix);
|
||||
for (auto& match : matches)
|
||||
{
|
||||
int trackletIndex = match.first;
|
||||
int detectionIndex = match.second;
|
||||
if (trackletIndex >= 0 && detectionIndex >= 0)
|
||||
{
|
||||
if (iouMatrix[match.first][match.second] <= 1-_iou_threshold) // iou_thrshold
|
||||
{
|
||||
sv::Box box;
|
||||
tgts.targets[detectionIndex].getBox(box);
|
||||
this->_tracklets[trackletIndex].age = 0;
|
||||
this->_tracklets[trackletIndex].hits++;
|
||||
this->_tracklets[trackletIndex].frame_id = tgts.frame_id;
|
||||
this->_tracklets[trackletIndex].bbox << box.x1+(box.x2-box.x1)/2, box.y1+(box.y2-box.y1)/2, box.x2-box.x1, box.y2-box.y1;
|
||||
tgts.targets[detectionIndex].tracked_id = this->_tracklets[trackletIndex].id;
|
||||
match_det[detectionIndex] = trackletIndex;
|
||||
}
|
||||
}
|
||||
}
|
||||
std::vector <vector<double>> ().swap(iouMatrix);
|
||||
for (int i=0; i<tgts.targets.size(); i++)
|
||||
{
|
||||
// cout << "match_det: index: " << i << " value: " << match_det[i] << endl;
|
||||
if (match_det[i] == -1)
|
||||
{
|
||||
// cout << "create new tracklet." << endl;
|
||||
sv::Box box;
|
||||
tgts.targets[i].getBox(box);
|
||||
Tracklet tracklet;
|
||||
tracklet.id = ++ this->_next_tracklet_id;
|
||||
tracklet.bbox << box.x1+(box.x2-box.x1)/2, (double)(box.y1+(box.y2-box.y1)/2), box.x2-box.x1, box.y2-box.y1;
|
||||
tracklet.age = 0;
|
||||
tracklet.hits = 1;
|
||||
tracklet.misses = 0;
|
||||
tracklet.frame_id = tgts.frame_id;
|
||||
tracklet.category_id = tgts.targets[i].category_id;
|
||||
tracklet.tentative = true;
|
||||
|
||||
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > new_motion = kf.initiate(tracklet.bbox);
|
||||
tracklet.mean = new_motion.first;
|
||||
tracklet.covariance = new_motion.second;
|
||||
|
||||
tgts.targets[i].tracked_id = this->_next_tracklet_id;
|
||||
tgts.targets[i].has_tid = true;
|
||||
this->_tracklets.push_back(tracklet);
|
||||
}
|
||||
else
|
||||
{
|
||||
sv::Box box;
|
||||
int track_id = match_det[i];
|
||||
tgts.targets[i].getBox(box);
|
||||
pair<Matrix<double, 8, 1>, Matrix<double, 8, 8> > updated = kf.update(this->_tracklets[track_id].mean, this->_tracklets[track_id].covariance, box);
|
||||
this->_tracklets[track_id].mean = updated.first;
|
||||
this->_tracklets[track_id].covariance = updated.second;
|
||||
}
|
||||
}
|
||||
|
||||
//sift tracklets
|
||||
for (auto& tracklet : this->_tracklets)
|
||||
{
|
||||
if (tracklet.hits >= _min_hits)
|
||||
{
|
||||
tracklet.tentative = false;
|
||||
}
|
||||
if ((tgts.frame_id-tracklet.frame_id <= _max_age) || (!tracklet.tentative && tracklet.frame_id == tgts.frame_id))
|
||||
{
|
||||
_new_tracklets.push_back(tracklet);
|
||||
}
|
||||
}
|
||||
_tracklets = _new_tracklets;
|
||||
std::vector <Tracklet> ().swap(_new_tracklets);
|
||||
}
|
||||
}
|
||||
|
||||
vector<Tracklet> SORT::getTracklets() const
|
||||
{
|
||||
return this->_tracklets;
|
||||
}
|
||||
|
||||
double SORT::_iou(Tracklet& tracklet, sv::Box& box)
|
||||
{
|
||||
double trackletX1 = tracklet.bbox(0)-tracklet.bbox(2)/2;
|
||||
double trackletY1 = tracklet.bbox(1)-tracklet.bbox(3)/2;
|
||||
double trackletX2 = tracklet.bbox(0) + tracklet.bbox(2)/2;
|
||||
double trackletY2 = tracklet.bbox(1) + tracklet.bbox(3)/2;
|
||||
|
||||
double detectionX1 = box.x1;
|
||||
double detectionY1 = box.y1;
|
||||
double detectionX2 = box.x2;
|
||||
double detectionY2 = box.y2;
|
||||
|
||||
double intersectionX1 = max(trackletX1, detectionX1);
|
||||
double intersectionY1 = max(trackletY1, detectionY1);
|
||||
double intersectionX2 = min(trackletX2, detectionX2);
|
||||
double intersectionY2 = min(trackletY2, detectionY2);
|
||||
|
||||
double w = (intersectionX2-intersectionX1 > 0.0) ? (intersectionX2-intersectionX1) : 0.0;
|
||||
double h = (intersectionY2-intersectionY1 > 0.0) ? (intersectionY2-intersectionY1) : 0.0;
|
||||
double intersectionArea = w * h;
|
||||
|
||||
double trackletArea = tracklet.bbox(2) * tracklet.bbox(3);
|
||||
|
||||
double detectionArea = (box.x2-box.x1) * (box.y2-box.y1);
|
||||
double unionArea = trackletArea + detectionArea - intersectionArea;
|
||||
double iou = (1 - intersectionArea / unionArea * 1.0);
|
||||
|
||||
return iou;
|
||||
}
|
||||
|
||||
// Function to find the minimum element in a vector
|
||||
double SORT::_findMin(const std::vector<double>& vec) {
|
||||
double minVal = std::numeric_limits<double>::max();
|
||||
for (double val : vec) {
|
||||
if (val < minVal) {
|
||||
minVal = val;
|
||||
}
|
||||
}
|
||||
return minVal;
|
||||
}
|
||||
|
||||
// Function to subtract the minimum value from each row of the cost matrix
|
||||
void SORT::_subtractMinFromRows(std::vector<std::vector<double>>& costMatrix) {
|
||||
for (auto& row : costMatrix) {
|
||||
double minVal = _findMin(row);
|
||||
for (double& val : row) {
|
||||
val -= minVal;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Function to subtract the minimum value from each column of the cost matrix
|
||||
void SORT::_subtractMinFromCols(std::vector<std::vector<double>>& costMatrix) {
|
||||
for (size_t col = 0; col < costMatrix[0].size(); ++col) {
|
||||
double minVal = std::numeric_limits<double>::max();
|
||||
for (size_t row = 0; row < costMatrix.size(); ++row) {
|
||||
if (costMatrix[row][col] < minVal) {
|
||||
minVal = costMatrix[row][col];
|
||||
}
|
||||
}
|
||||
for (size_t row = 0; row < costMatrix.size(); ++row) {
|
||||
costMatrix[row][col] -= minVal;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Function to find a matching using the Hungarian algorithm
|
||||
vector<pair<int, int> > SORT::_hungarian(vector<vector<double> > costMatrix)
|
||||
{
|
||||
size_t numRows = costMatrix.size();
|
||||
size_t numCols = costMatrix[0].size();
|
||||
|
||||
//transpose the matrix if necessary
|
||||
const bool transposed = numCols > numRows;
|
||||
if (transposed) {
|
||||
vector<vector<double>> transposedMatrix(numCols, vector<double>(numRows));
|
||||
for (int i = 0; i < numRows; i++)
|
||||
{
|
||||
for (int j = 0; j < numCols; j++)
|
||||
{
|
||||
transposedMatrix[j][i] = costMatrix[i][j];
|
||||
}
|
||||
}
|
||||
costMatrix = transposedMatrix;
|
||||
swap(numRows, numCols);
|
||||
}
|
||||
// Determine the larger dimension for matching
|
||||
size_t maxDim = std::max(numRows, numCols);
|
||||
|
||||
// Create a square cost matrix by padding with zeros if necessary
|
||||
std::vector<std::vector<double>> squareMatrix(maxDim, std::vector<double>(maxDim, 0.0));
|
||||
for (size_t row = 0; row < numRows; ++row) {
|
||||
for (size_t col = 0; col < numCols; ++col) {
|
||||
squareMatrix[row][col] = costMatrix[row][col];
|
||||
}
|
||||
}
|
||||
|
||||
// Subtract the minimum value from each row and column
|
||||
_subtractMinFromRows(squareMatrix);
|
||||
_subtractMinFromCols(squareMatrix);
|
||||
|
||||
// Initialize the assignment vectors with -1 values
|
||||
std::vector<int> rowAssignment(maxDim, -1);
|
||||
std::vector<int> colAssignment(maxDim, -1);
|
||||
|
||||
// Perform the matching
|
||||
for (size_t row = 0; row < maxDim; ++row) {
|
||||
std::vector<bool> visitedCols(maxDim, false);
|
||||
for (size_t col = 0; col < maxDim; ++col) {
|
||||
if (squareMatrix[row][col] == 0 && colAssignment[col] == -1) {
|
||||
rowAssignment[row] = col;
|
||||
colAssignment[col] = row;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Convert the assignment vectors to pair<int, int> format
|
||||
std::vector<std::pair<int, int>> assignmentPairs;
|
||||
for (size_t row = 0; row < numRows; ++row) {
|
||||
int col = rowAssignment[row];
|
||||
//if (col != -1) {
|
||||
// assignmentPairs.emplace_back(row, col);
|
||||
// }
|
||||
if (col != -1) {
|
||||
if (col >= numCols) {
|
||||
col = -1;
|
||||
}
|
||||
assignmentPairs.emplace_back(row, col);
|
||||
}
|
||||
}
|
||||
if (transposed) {
|
||||
for (auto& assignment : assignmentPairs)
|
||||
{
|
||||
swap(assignment.first, assignment.second);
|
||||
}
|
||||
}
|
||||
return assignmentPairs;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -20,9 +20,9 @@
|
|||
uint32_t GX40GimbalDriver::setGimabalPos(const AMOV_GIMBAL_POS_T &pos)
|
||||
{
|
||||
carrierStateMutex.lock();
|
||||
targetPos[0] = (int16_t)(pos.roll / 0.01f);
|
||||
targetPos[0] = (int16_t)(-pos.roll / 0.01f);
|
||||
targetPos[1] = (int16_t)(-pos.pitch / 0.01f);
|
||||
targetPos[2] = (int16_t)(pos.yaw / 0.01f);
|
||||
targetPos[2] = (int16_t)(-pos.yaw / 0.01f);
|
||||
carrierStateMutex.unlock();
|
||||
return pack(GX40::GIMBAL_CMD_MODE_EULER, nullptr, 0);
|
||||
}
|
||||
|
|
@ -38,9 +38,9 @@ uint32_t GX40GimbalDriver::setGimabalPos(const AMOV_GIMBAL_POS_T &pos)
|
|||
uint32_t GX40GimbalDriver::setGimabalSpeed(const AMOV_GIMBAL_POS_T &speed)
|
||||
{
|
||||
carrierStateMutex.lock();
|
||||
targetPos[0] = (int16_t)(speed.roll / 0.1f);
|
||||
targetPos[0] = (int16_t)(-speed.roll / 0.1f);
|
||||
targetPos[1] = (int16_t)(-speed.pitch / 0.1f);
|
||||
targetPos[2] = (int16_t)(speed.yaw / 0.1f);
|
||||
targetPos[2] = (int16_t)(-speed.yaw / 0.1f);
|
||||
carrierStateMutex.unlock();
|
||||
|
||||
return pack(GX40::GIMBAL_CMD_MODE_FOLLOW, nullptr, 0);
|
||||
|
|
|
|||
|
|
@ -509,50 +509,6 @@ void sv::Gimbal::setAngleRateEuler(double roll_rate, double pitch_rate, double y
|
|||
temp.yaw = yaw_rate;
|
||||
pdevTemp->setGimabalSpeed(temp);
|
||||
}
|
||||
void sv::Gimbal::attitudeCorrection(const GimbalQuaternionT &quaterion,
|
||||
const GimbalVelocityT &speed,
|
||||
const GimbalVelocityT &acc, void *extenData)
|
||||
{
|
||||
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
|
||||
AMOV_GIMBAL_QUATERNION_T temp_q;
|
||||
temp_q.q0 = quaterion.q0;
|
||||
temp_q.q1 = quaterion.q1;
|
||||
temp_q.q2 = quaterion.q2;
|
||||
temp_q.q3 = quaterion.q3;
|
||||
|
||||
AMOV_GIMBAL_VELOCITY_T temp_v1, temp_v2;
|
||||
temp_v1.x = speed.x;
|
||||
temp_v1.y = speed.y;
|
||||
temp_v1.z = speed.z;
|
||||
|
||||
temp_v2.x = acc.x;
|
||||
temp_v2.y = acc.y;
|
||||
temp_v2.z = acc.z;
|
||||
|
||||
pdevTemp->attitudeCorrection(temp_q, temp_v1, temp_v2, extenData);
|
||||
}
|
||||
|
||||
void sv::Gimbal::attitudeCorrection(const GimbalPosT &pos,
|
||||
const GimbalVelocityT &speed,
|
||||
const GimbalVelocityT &acc, void *extenData)
|
||||
{
|
||||
amovGimbal::gimbal *pdevTemp = (amovGimbal::gimbal *)this->dev;
|
||||
AMOV_GIMBAL_VELOCITY_T temp_v1, temp_v2;
|
||||
temp_v1.x = speed.x;
|
||||
temp_v1.y = speed.y;
|
||||
temp_v1.z = speed.z;
|
||||
|
||||
temp_v2.x = acc.x;
|
||||
temp_v2.y = acc.y;
|
||||
temp_v2.z = acc.z;
|
||||
|
||||
AMOV_GIMBAL_POS_T temp_p;
|
||||
temp_p.pitch = pos.pitch;
|
||||
temp_p.yaw = pos.yaw;
|
||||
temp_p.roll = pos.roll;
|
||||
|
||||
pdevTemp->attitudeCorrection(temp_p, temp_v1, temp_v2, extenData);
|
||||
}
|
||||
|
||||
sv::Gimbal::~Gimbal()
|
||||
{
|
||||
|
|
|
|||
|
|
@ -78,28 +78,6 @@ namespace sv
|
|||
FLOWCONTROL_HARDWARE = 2,
|
||||
};
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double q0;
|
||||
double q1;
|
||||
double q2;
|
||||
double q3;
|
||||
} GimbalQuaternionT;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double x; // or N
|
||||
double y; // or E
|
||||
double z; // or UP
|
||||
} GimbalVelocityT;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double yaw;
|
||||
double roll;
|
||||
double pitch;
|
||||
} GimbalPosT;
|
||||
|
||||
static inline void emptyCallback(double &frameAngleRoll, double &frameAnglePitch, double &frameAngleYaw,
|
||||
double &imuAngleRoll, double &imuAnglePitch, double &imuAngleYaw,
|
||||
double &fovX, double &fovY)
|
||||
|
|
@ -187,12 +165,6 @@ namespace sv
|
|||
bool takePhoto();
|
||||
bool takeVideo(int state);
|
||||
int getVideoState();
|
||||
void attitudeCorrection(const GimbalQuaternionT &quaterion,
|
||||
const GimbalVelocityT &speed,
|
||||
const GimbalVelocityT &acc, void *extenData);
|
||||
void attitudeCorrection(const GimbalPosT &pos,
|
||||
const GimbalVelocityT &speed,
|
||||
const GimbalVelocityT &acc, void *extenData);
|
||||
|
||||
//! Set gimbal angles
|
||||
/*!
|
||||
|
|
|
|||
|
|
@ -8,14 +8,14 @@
|
|||
#include <opencv2/tracking.hpp>
|
||||
#include <string>
|
||||
#include <chrono>
|
||||
#include <Eigen/Dense>
|
||||
|
||||
//#include <Eigen/Dense>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
namespace sv {
|
||||
|
||||
|
||||
class SORT;
|
||||
class BYTETracker;
|
||||
|
||||
|
||||
class MultipleObjectTracker : public CameraAlgorithm
|
||||
{
|
||||
|
|
@ -24,7 +24,7 @@ public:
|
|||
~MultipleObjectTracker();
|
||||
|
||||
void init(CommonObjectDetector* detector_);
|
||||
sv::TargetsInFrame track(cv::Mat img_, TargetsInFrame& tgts_);
|
||||
void track(cv::Mat img_, TargetsInFrame& tgts_);
|
||||
|
||||
private:
|
||||
void _load();
|
||||
|
|
@ -39,12 +39,70 @@ private:
|
|||
double _iou_thres;
|
||||
int _max_age;
|
||||
int _min_hits;
|
||||
int _frame_rate;
|
||||
int _track_buffer;
|
||||
|
||||
SORT* _sort_impl;
|
||||
BYTETracker* _bytetrack_impl;
|
||||
CommonObjectDetector* _detector;
|
||||
};
|
||||
|
||||
|
||||
// define the tracklet struct to store the tracked objects.
|
||||
struct Tracklet
|
||||
{
|
||||
/* data */
|
||||
public:
|
||||
Eigen::Vector4d bbox; // double x, y, w, h;
|
||||
int id = 0;
|
||||
int age;
|
||||
int hits;
|
||||
int misses;
|
||||
int frame_id = 0;
|
||||
int category_id;
|
||||
bool tentative;
|
||||
std::vector<double> features;
|
||||
Eigen::Matrix<double, 8, 1> mean;
|
||||
Eigen::Matrix<double, 8, 8> covariance;
|
||||
};
|
||||
|
||||
|
||||
class KalmanFilter {
|
||||
public:
|
||||
KalmanFilter();
|
||||
~KalmanFilter();
|
||||
std::pair<Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8> > initiate(Eigen::Vector4d &bbox);
|
||||
std::pair<Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8> > update(Eigen::Matrix<double, 8, 1> mean, Eigen::Matrix<double, 8, 8> covariances, Box &box);
|
||||
std::pair<Eigen::Matrix<double, 8, 1>, Eigen::Matrix<double, 8, 8> > predict(Eigen::Matrix<double, 8, 1> mean, Eigen::Matrix<double, 8, 8> covariances);
|
||||
std::pair<Eigen::Matrix<double, 4, 1>, Eigen::Matrix<double, 4, 4> > project(Eigen::Matrix<double, 8, 1> mean, Eigen::Matrix<double, 8, 8> covariances);
|
||||
private:
|
||||
Eigen::Matrix<double, 8, 8> _F;
|
||||
Eigen::Matrix<double, 4, 8> _H;
|
||||
Eigen::Matrix<double, 9, 1> _chi2inv95;
|
||||
double _std_weight_position;
|
||||
double _std_weight_vel;
|
||||
};
|
||||
|
||||
|
||||
class SORT {
|
||||
public:
|
||||
SORT(double iou_threshold, int max_age, int min_hits): _iou_threshold(iou_threshold), _max_age(max_age), _min_hits(min_hits), _next_tracklet_id(0) {};
|
||||
~SORT();
|
||||
void update(TargetsInFrame &tgts);
|
||||
std::vector<Tracklet> getTracklets() const;
|
||||
private:
|
||||
double _iou(Tracklet &tracklet, Box &box);
|
||||
std::vector<std::pair<int,int>> _hungarian(std::vector<std::vector<double>> costMatrix);
|
||||
double _findMin(const std::vector<double>& vec);
|
||||
void _subtractMinFromRows(std::vector<std::vector<double>>& costMatrix);
|
||||
void _subtractMinFromCols(std::vector<std::vector<double>>& costMatrix);
|
||||
//bool _augment(const std::vector<std::vector<double>>& costMatrix, int row, std::vector<int>& rowMatch, std::vector<int>& colMatch, std::vector<bool>& visited);
|
||||
|
||||
double _iou_threshold;
|
||||
int _max_age;
|
||||
int _min_hits;
|
||||
int _next_tracklet_id;
|
||||
std::vector <Tracklet> _tracklets;
|
||||
std::vector <Tracklet> _new_tracklets;
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -126,9 +126,7 @@
|
|||
"reid_match_thres": 2.0,
|
||||
"iou_thres": 0.5,
|
||||
"max_age": 10,
|
||||
"min_hits": 3,
|
||||
"frame_rate": 30,
|
||||
"track_buffer": 30
|
||||
"min_hits": 3
|
||||
},
|
||||
"LandingMarkerDetector": {
|
||||
"labels": ["h"],
|
||||
|
|
|
|||
|
|
@ -126,9 +126,7 @@
|
|||
"reid_match_thres": 2.0,
|
||||
"iou_thres": 0.5,
|
||||
"max_age": 10,
|
||||
"min_hits": 3,
|
||||
"frame_rate": 30,
|
||||
"track_buffer": 30
|
||||
"min_hits": 3
|
||||
},
|
||||
"LandingMarkerDetector": {
|
||||
"labels": ["x", "h"],
|
||||
|
|
|
|||
|
|
@ -125,9 +125,8 @@
|
|||
"reid_num_samples": 10,
|
||||
"reid_match_thres": 2.0,
|
||||
"iou_thres": 0.5,
|
||||
"min_hits": 3,
|
||||
"frame_rate": 30,
|
||||
"track_buffer": 30
|
||||
"max_age": 10,
|
||||
"min_hits": 3
|
||||
},
|
||||
"LandingMarkerDetector": {
|
||||
"labels": ["x", "h"],
|
||||
|
|
|
|||
|
|
@ -126,9 +126,7 @@
|
|||
"reid_match_thres": 2.0,
|
||||
"iou_thres": 0.5,
|
||||
"max_age": 10,
|
||||
"min_hits": 3,
|
||||
"frame_rate": 30,
|
||||
"track_buffer": 30
|
||||
"min_hits": 3
|
||||
},
|
||||
"LandingMarkerDetector": {
|
||||
"labels": ["x", "h"],
|
||||
|
|
|
|||
|
|
@ -126,9 +126,7 @@
|
|||
"reid_match_thres": 2.0,
|
||||
"iou_thres": 0.5,
|
||||
"max_age": 10,
|
||||
"min_hits": 3,
|
||||
"frame_rate": 30,
|
||||
"track_buffer": 30
|
||||
"min_hits": 3
|
||||
},
|
||||
"LandingMarkerDetector": {
|
||||
"labels": ["x", "h"],
|
||||
|
|
|
|||
|
|
@ -126,9 +126,7 @@
|
|||
"reid_match_thres": 2.0,
|
||||
"iou_thres": 0.5,
|
||||
"max_age": 10,
|
||||
"min_hits": 3,
|
||||
"frame_rate": 30,
|
||||
"track_buffer": 30
|
||||
"min_hits": 3
|
||||
},
|
||||
"LandingMarkerDetector": {
|
||||
"labels": ["x", "h"],
|
||||
|
|
|
|||
|
|
@ -126,9 +126,7 @@
|
|||
"reid_match_thres": 2.0,
|
||||
"iou_thres": 0.5,
|
||||
"max_age": 10,
|
||||
"min_hits": 3,
|
||||
"frame_rate": 30,
|
||||
"track_buffer": 30
|
||||
"min_hits": 3
|
||||
},
|
||||
"LandingMarkerDetector": {
|
||||
"labels": ["x", "h"],
|
||||
|
|
|
|||
|
|
@ -126,9 +126,7 @@
|
|||
"reid_match_thres": 2.0,
|
||||
"iou_thres": 0.5,
|
||||
"max_age": 10,
|
||||
"min_hits": 3,
|
||||
"frame_rate": 30,
|
||||
"track_buffer": 30
|
||||
"min_hits": 3
|
||||
},
|
||||
"LandingMarkerDetector": {
|
||||
"labels": ["x", "h"],
|
||||
|
|
|
|||
|
|
@ -126,9 +126,7 @@
|
|||
"reid_match_thres": 2.0,
|
||||
"iou_thres": 0.5,
|
||||
"max_age": 10,
|
||||
"min_hits": 3,
|
||||
"frame_rate": 30,
|
||||
"track_buffer": 30
|
||||
"min_hits": 3
|
||||
},
|
||||
"LandingMarkerDetector": {
|
||||
"labels": ["x", "h"],
|
||||
|
|
|
|||
|
|
@ -126,9 +126,7 @@
|
|||
"reid_match_thres": 2.0,
|
||||
"iou_thres": 0.5,
|
||||
"max_age": 10,
|
||||
"min_hits": 3,
|
||||
"frame_rate": 30,
|
||||
"track_buffer": 30
|
||||
"min_hits": 3
|
||||
},
|
||||
"LandingMarkerDetector": {
|
||||
"labels": ["x", "h"],
|
||||
|
|
|
|||
|
|
@ -1,76 +0,0 @@
|
|||
#include <iostream>
|
||||
#include <string>
|
||||
#include <experimental/filesystem>
|
||||
// 包含SpireCV SDK头文件
|
||||
#include <sv_world.h>
|
||||
|
||||
using namespace std;
|
||||
namespace fs = std::experimental::filesystem;
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
// 实例化
|
||||
sv::CommonObjectDetector cod;
|
||||
// 手动导入相机参数，如果使用Amov的G1等吊舱或相机，则可以忽略该步骤，将自动下载相机参数文件
|
||||
cod.loadCameraParams(sv::get_home() + "/SpireCV/confs/calib_webcam_1280x720.yaml");
|
||||
cod.loadAlgorithmParams(sv::get_home() + "/SpireCV/confs/sv_algorithm_params.json");
|
||||
sv::MultipleObjectTracker mot;
|
||||
// 手动导入相机参数，如果使用Amov的G1等吊舱或相机，则可以忽略该步骤，将自动下载相机参数文件
|
||||
mot.loadCameraParams(sv::get_home() + "/SpireCV/confs/calib_webcam_1280x720.yaml");
|
||||
mot.loadAlgorithmParams(sv::get_home() + "/SpireCV/confs/sv_algorithm_params.json");
|
||||
mot.init(&cod);
|
||||
|
||||
// 打开摄像头
|
||||
/*
|
||||
sv::Camera cap;
|
||||
cap.setWH(mot.image_width, mot.image_height);
|
||||
cap.setFps(30);
|
||||
cap.open(sv::CameraType::V4L2CAM, 0); // CameraID 0
|
||||
*/
|
||||
std::string mot17_folder_path = sv::get_home()+"/SpireCV/dataset/MOT17/train/";
|
||||
std::string pred_file_path = sv::get_home()+"/SpireCV/dataset/pred_mot17/data/";
|
||||
for (auto & seq_path : std::experimental::filesystem::directory_iterator(mot17_folder_path))
|
||||
{
|
||||
// mkdir pred dirs and touch pred_files
|
||||
string pred_file = pred_file_path + seq_path.path().filename().string() + ".txt";
|
||||
fs::create_directories(pred_file_path);
|
||||
std::ofstream file(pred_file);
|
||||
// listdir seqence images
|
||||
string seq_image_paths = mot17_folder_path + seq_path.path().filename().string() + "/img1";
|
||||
// cout << seq_image_paths <<endl;
|
||||
std::vector<std::string> seq_image_file_path;
|
||||
cv::glob(seq_image_paths, seq_image_file_path);
|
||||
|
||||
//eval MOT algorithms
|
||||
cv::Mat img;
|
||||
int frame_id = 0;
|
||||
while (frame_id < seq_image_file_path.size())
|
||||
{
|
||||
img = cv::imread(seq_image_file_path[frame_id]);
|
||||
// 实例化SpireCV的 单帧检测结果 接口类 TargetsInFrame
|
||||
sv::TargetsInFrame tgts(frame_id++);
|
||||
// 读取一帧图像到img
|
||||
//cap.read(img);
|
||||
//cv::resize(img, img, cv::Size(mot.image_width, mot.image_height));
|
||||
|
||||
// 执行通用目标检测
|
||||
sv::TargetsInFrame person_tgts = mot.track(img, tgts);
|
||||
// 可视化检测结果，叠加到img上
|
||||
sv::drawTargetsInFrame(img, person_tgts);
|
||||
// printf(" Frame Size (width, height) = (%d, %d)\n", tgts.width, tgts.height);
|
||||
for (auto target : person_tgts.targets)
|
||||
{
|
||||
int center_x = int(target.cx * tgts.width);
|
||||
int center_y = int(target.cy * tgts.height);
|
||||
int width = int(target.w * tgts.width);
|
||||
int height = int(target.h * tgts.height);
|
||||
double conf = target.score;
|
||||
file << frame_id << ","<< target.tracked_id << "," << center_x - width / 2 << "," << center_y - height / 2 << "," << width << "," << height << "," << conf << "," << "-1,-1,-1" << endl;
|
||||
// file << frame_id << ","<< target.tracked_id << "," << center_x << "," << center_y << "," << width << "," << height << "," << conf << "," << "-1,-1,-1" << endl;
|
||||
}
|
||||
cv::imshow("img", img);
|
||||
cv::waitKey(10);
|
||||
}
|
||||
file.close();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -34,9 +34,9 @@ int main(int argc, char *argv[]) {
|
|||
cv::resize(img, img, cv::Size(mot.image_width, mot.image_height));
|
||||
|
||||
// 执行通用目标检测
|
||||
sv::TargetsInFrame person_tgts = mot.track(img, tgts);
|
||||
mot.track(img, tgts);
|
||||
// 可视化检测结果，叠加到img上
|
||||
sv::drawTargetsInFrame(img, person_tgts);
|
||||
sv::drawTargetsInFrame(img, tgts);
|
||||
|
||||
// 显示检测结果img
|
||||
cv::imshow("img", img);
|
||||
|
|
|
|||
|
|
@ -1,9 +0,0 @@
|
|||
python3 run_mot_challenge.py \
|
||||
--BENCHMARK MOT17 \
|
||||
--GT_FOLDER ../../../dataset/MOT17/train \
|
||||
--TRACKERS_FOLDER ../../../dataset/pred_mot17 \
|
||||
--SKIP_SPLIT_FOL True \
|
||||
--SEQMAP_FILE ../../../dataset/MOT17/val_seqmap.txt \
|
||||
--TRACKERS_TO_EVAL '' \
|
||||
--METRICS HOTA CLEAR Identity \
|
||||
#--show
|
||||
|
|
@ -1,93 +0,0 @@
|
|||
|
||||
""" run_mot_challenge.py
|
||||
|
||||
Run example:
|
||||
run_mot_challenge.py --USE_PARALLEL False --METRICS Hota --TRACKERS_TO_EVAL Lif_T
|
||||
|
||||
Command Line Arguments: Defaults, # Comments
|
||||
Eval arguments:
|
||||
'USE_PARALLEL': False,
|
||||
'NUM_PARALLEL_CORES': 8,
|
||||
'BREAK_ON_ERROR': True,
|
||||
'PRINT_RESULTS': True,
|
||||
'PRINT_ONLY_COMBINED': False,
|
||||
'PRINT_CONFIG': True,
|
||||
'TIME_PROGRESS': True,
|
||||
'OUTPUT_SUMMARY': True,
|
||||
'OUTPUT_DETAILED': True,
|
||||
'PLOT_CURVES': True,
|
||||
Dataset arguments:
|
||||
'GT_FOLDER': os.path.join(code_path, 'data/gt/mot_challenge/'), # Location of GT data
|
||||
'TRACKERS_FOLDER': os.path.join(code_path, 'data/trackers/mot_challenge/'), # Trackers location
|
||||
'OUTPUT_FOLDER': None, # Where to save eval results (if None, same as TRACKERS_FOLDER)
|
||||
'TRACKERS_TO_EVAL': None, # Filenames of trackers to eval (if None, all in folder)
|
||||
'CLASSES_TO_EVAL': ['pedestrian'], # Valid: ['pedestrian']
|
||||
'BENCHMARK': 'MOT17', # Valid: 'MOT17', 'MOT16', 'MOT20', 'MOT15'
|
||||
'SPLIT_TO_EVAL': 'train', # Valid: 'train', 'test', 'all'
|
||||
'INPUT_AS_ZIP': False, # Whether tracker input files are zipped
|
||||
'PRINT_CONFIG': True, # Whether to print current config
|
||||
'DO_PREPROC': True, # Whether to perform preprocessing (never done for 2D_MOT_2015)
|
||||
'TRACKER_SUB_FOLDER': 'data', # Tracker files are in TRACKER_FOLDER/tracker_name/TRACKER_SUB_FOLDER
|
||||
'OUTPUT_SUB_FOLDER': '', # Output files are saved in OUTPUT_FOLDER/tracker_name/OUTPUT_SUB_FOLDER
|
||||
Metric arguments:
|
||||
'METRICS': ['HOTA', 'CLEAR', 'Identity', 'VACE']
|
||||
"""
|
||||
|
||||
import sys
|
||||
import os
|
||||
import argparse
|
||||
from multiprocessing import freeze_support
|
||||
|
||||
sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
|
||||
import trackeval # noqa: E402
|
||||
|
||||
if __name__ == '__main__':
|
||||
freeze_support()
|
||||
|
||||
# Command line interface:
|
||||
default_eval_config = trackeval.Evaluator.get_default_eval_config()
|
||||
default_eval_config['DISPLAY_LESS_PROGRESS'] = False
|
||||
default_dataset_config = trackeval.datasets.MotChallenge2DBox.get_default_dataset_config()
|
||||
default_metrics_config = {'METRICS': ['HOTA', 'CLEAR', 'Identity'], 'THRESHOLD': 0.5}
|
||||
config = {**default_eval_config, **default_dataset_config, **default_metrics_config} # Merge default configs
|
||||
parser = argparse.ArgumentParser()
|
||||
for setting in config.keys():
|
||||
if type(config[setting]) == list or type(config[setting]) == type(None):
|
||||
parser.add_argument("--" + setting, nargs='+')
|
||||
else:
|
||||
parser.add_argument("--" + setting)
|
||||
args = parser.parse_args().__dict__
|
||||
args['SEQMAP_FILE'] = args['SEQMAP_FILE'][0]
|
||||
# args['SEQ_INFO']={'MOT17-02-DPM':600}
|
||||
for setting in args.keys():
|
||||
if args[setting] is not None:
|
||||
if type(config[setting]) == type(True):
|
||||
if args[setting] == 'True':
|
||||
x = True
|
||||
elif args[setting] == 'False':
|
||||
x = False
|
||||
else:
|
||||
raise Exception('Command line parameter ' + setting + 'must be True or False')
|
||||
elif type(config[setting]) == type(1):
|
||||
x = int(args[setting])
|
||||
elif type(args[setting]) == type(None):
|
||||
x = None
|
||||
elif setting == 'SEQ_INFO':
|
||||
x = dict(zip(args[setting], [None]*len(args[setting])))
|
||||
else:
|
||||
x = args[setting]
|
||||
config[setting] = x
|
||||
eval_config = {k: v for k, v in config.items() if k in default_eval_config.keys()}
|
||||
dataset_config = {k: v for k, v in config.items() if k in default_dataset_config.keys()}
|
||||
metrics_config = {k: v for k, v in config.items() if k in default_metrics_config.keys()}
|
||||
|
||||
# Run code
|
||||
evaluator = trackeval.Evaluator(eval_config)
|
||||
dataset_list = [trackeval.datasets.MotChallenge2DBox(dataset_config)]
|
||||
metrics_list = []
|
||||
for metric in [trackeval.metrics.HOTA, trackeval.metrics.CLEAR, trackeval.metrics.Identity, trackeval.metrics.VACE]:
|
||||
if metric.get_name() in metrics_config['METRICS']:
|
||||
metrics_list.append(metric(metrics_config))
|
||||
if len(metrics_list) == 0:
|
||||
raise Exception('No metrics selected for evaluation')
|
||||
evaluator.evaluate(dataset_list, metrics_list)
|
||||
|
|
@ -1,23 +0,0 @@
|
|||
import os
|
||||
import os.path as osp
|
||||
|
||||
|
||||
|
||||
def makedirs(path):
|
||||
if not osp.exists(path):
|
||||
os.makedirs(path)
|
||||
else:
|
||||
print('this file is already exists.')
|
||||
|
||||
|
||||
if __name__=="__main__":
|
||||
dst_path = '/home/bitwrj/SpireCV/dataset/MOT17'
|
||||
mode = 'train'
|
||||
dst_file_name = 'val_seqmap.txt'
|
||||
dst_file = osp.join(dst_path,dst_file_name)
|
||||
test_files = os.listdir(osp.join(dst_path,mode))
|
||||
test_files.sort()
|
||||
with open(dst_file, 'w+') as f:
|
||||
f.writelines('name\n')
|
||||
for test_file in test_files:
|
||||
f.writelines(test_file+'\n')
|
||||
|
|
@ -1,20 +0,0 @@
|
|||
#!/bin/sh
|
||||
|
||||
sudo apt install -y libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev
|
||||
sudo apt install -y libgstreamer-plugins-bad1.0-dev gstreamer1.0-plugins-base
|
||||
sudo apt install -y gstreamer1.0-plugins-good gstreamer1.0-plugins-bad
|
||||
sudo apt install -y gstreamer1.0-plugins-ugly gstreamer1.0-libav gstreamer1.0-doc
|
||||
sudo apt install -y gstreamer1.0-tools gstreamer1.0-x gstreamer1.0-alsa
|
||||
sudo apt install -y gstreamer1.0-gl gstreamer1.0-gtk3 gstreamer1.0-qt5
|
||||
sudo apt install -y gstreamer1.0-pulseaudio
|
||||
sudo apt install -y gtk-doc-tools
|
||||
sudo apt install -y libeigen3-dev libfmt-dev v4l-utils
|
||||
|
||||
git clone https://gitee.com/jario-jin/gst-rtsp-server-b18.git
|
||||
cd gst-rtsp-server-b18
|
||||
./autogen.sh
|
||||
make
|
||||
sudo make install
|
||||
cd ..
|
||||
sudo rm -r gst-rtsp-server-b18
|
||||
|
||||
|
|
@ -1,6 +0,0 @@
|
|||
#!/bin/sh
|
||||
|
||||
sudo apt-get install -y libmfx1 libmfx-tools libva-dev libmfx-dev intel-media-va-driver-non-free vainfo va-driver-all vdpau-va-driver
|
||||
echo "export LIBVA_DRIVER_NAME=i965" >> ~/.bashrc
|
||||
export LIBVA_DRIVER_NAME=i965
|
||||
|
||||
|
|
@ -60,19 +60,7 @@ cmake -D CMAKE_BUILD_TYPE=Release \
|
|||
-D WITH_CUDA=OFF \
|
||||
-D OPENCV_ENABLE_NONFREE=ON \
|
||||
-D CMAKE_INSTALL_PREFIX=/usr/local \
|
||||
-D OPENCV_EXTRA_MODULES_PATH=../../opencv_contrib-4.7.0/modules \
|
||||
-D BUILD_PNG=ON \
|
||||
-D BUILD_JASPER=ON \
|
||||
-D BUILD_JPEG=ON \
|
||||
-D BUILD_TIFF=ON \
|
||||
-D BUILD_ZLIB=ON \
|
||||
-D WITH_JPEG=ON \
|
||||
-D WITH_PNG=ON \
|
||||
-D WITH_JASPER=ON \
|
||||
-D WITH_TIFF=ON \
|
||||
-D WITH_TBB=ON \
|
||||
-D WITH_ZLIB=ON \
|
||||
-D WITH_OPENCL=ON ..
|
||||
-D OPENCV_EXTRA_MODULES_PATH=../../opencv_contrib-4.7.0/modules ..
|
||||
|
||||
make -j2
|
||||
sudo make install
|
||||
|
|
@ -116,7 +116,7 @@ void Camera::openImpl()
|
|||
camera_url << "rtspsrc location = rtsp://user:0000@" << this->_ip << ":" << this->_port
|
||||
<< "/cam/realmonitor?channel=1&subtype=0 latency=100 ! \
|
||||
application/x-rtp,media=video ! rtph265depay ! parsebin ! \
|
||||
nvv4l2decoder ! nvvidconv ! \
|
||||
nvv4l2decoder ! nvvidconv flip-method=4 ! \
|
||||
video/x-raw,format=(string)BGRx,width=(int)"
|
||||
<< this->_width << ",height=(int)" << this->_height << " ! videoconvert ! video/x-raw,format=(string)BGR ! \
|
||||
appsink sync=false";
|
||||
|
|
@ -159,23 +159,13 @@ void Camera::openImpl()
|
|||
this->_fps = 30;
|
||||
}
|
||||
|
||||
#if defined(PLATFORM_X86_CUDA)
|
||||
#if defined(PLATFORM_X86_CUDA) || defined(PLATFORM_X86_INTEL)
|
||||
sprintf(pipe, "rtspsrc location=%s?W=%d&H=%d&FPS=%d latency=100 ! \
|
||||
application/x-rtp,media=video ! rtph264depay ! parsebin ! avdec_h264 ! \
|
||||
videoconvert ! appsink sync=false",
|
||||
this->_rtsp_url.c_str(), this->_width, this->_height, this->_fps);
|
||||
this->_cap.open(pipe, cv::CAP_GSTREAMER);
|
||||
#endif
|
||||
#if defined(PLATFORM_X86_INTEL)
|
||||
sprintf(pipe, "rtspsrc location=%s?W=%d&H=%d&FPS=%d latency=100 ! \
|
||||
application/x-rtp,media=video ! rtph264depay ! parsebin ! vaapih264dec ! \
|
||||
vaapipostproc ! video/x-raw,format=YV12,framerate=25/1 ! videoconvert ! videorate ! video/x-raw,framerate=25/1 ! \
|
||||
appsink sync=false",
|
||||
this->_rtsp_url.c_str(), this->_width, this->_height, this->_fps);
|
||||
this->_cap.open(pipe, cv::CAP_GSTREAMER);
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef PLATFORM_JETSON
|
||||
sprintf(pipe, "rtspsrc location=%s?W=%d&H=%d&FPS=%d latency=100 ! application/x-rtp,media=video ! rtph264depay ! parsebin ! nvv4l2decoder enable-max-performancegst=1 ! nvvidconv ! video/x-raw,format=(string)BGRx ! videoconvert ! appsink sync=false", this->_rtsp_url.c_str(), this->_width, this->_height, this->_fps);
|
||||
this->_cap.open(pipe, cv::CAP_GSTREAMER);
|
||||
|
|
|
|||
Loading…
Reference in New Issue