mmr/tkDNN
Archived
1
0
Fork 0
Code Issues 46 Pull Requests 12 Releases 4 Activity

Improve CenterTrack.

Browse Source 
Signed-off-by: Davide Sapienza <sapienza.dav@gmail.com>
This commit is contained in:
Davide Sapienza
2021-05-04 17:59:20 +02:00
parent f8327e2dac
commit 0dc96d2a9e
6 changed files with 397 additions and 396 deletions
Download Patch File Download Diff File Expand all files Collapse all files
demo/demo/demo3D.cpp
+1 -9
View File
@@ -96,8 +96,6 @@ int main(int argc, char *argv[]) {
int h = cap.get(cv::CAP_PROP_FRAME_HEIGHT);
resultVideo.open("result.mp4", cv::VideoWriter::fourcc('M','P','4','V'), 30, cv::Size(w, h));
}
cv::Size sz_resize = cv::Size(512,512);
std::vector<cv::Size> sz_orig;
cv::Mat frame;
if(show)
cv::namedWindow("detection", cv::WINDOW_NORMAL);
@@ -108,15 +106,11 @@ int main(int argc, char *argv[]) {
while(gRun) {
batch_dnn_input.clear();
batch_frame.clear();
sz_orig.clear();
for(int bi=0; bi< n_batch; ++bi){
cap >> frame;
if(!frame.data)
break;
sz_orig.push_back(frame.size());
if(calibs.size() != 0)
resize(frame, frame, sz_resize);
batch_frame.push_back(frame);
// this will be resized to the net format
@@ -126,13 +120,11 @@ int main(int argc, char *argv[]) {
break;
//inference
detNN->update(batch_dnn_input, n_batch, false, nullptr, false, sz_orig);
detNN->update(batch_dnn_input, n_batch, false, nullptr, false);
detNN->draw(batch_frame);
if(show){
for(int bi=0; bi< n_batch; ++bi){
if(calibs.size() != 0)
resize(batch_frame[bi], batch_frame[bi], sz_orig[bi]);
cv::imshow("detection", batch_frame[bi]);
cv::waitKey(1);
}
include/tkDNN/CenternetDetection3D.h
+8 -5
View File
@@ -27,6 +27,8 @@ private:
tk::dnn::dataDim_t dim_dep;
tk::dnn::dataDim_t dim_rot;
tk::dnn::dataDim_t dim_dim;
std::vector<cv::Mat> inputCalibs;
float *topk_scores;
int *topk_inds_;
float *topk_ys_;
@@ -58,32 +60,33 @@ private:
dnnType *input;
#endif
cv::Mat r;
cv::Mat calibs;
float *d_ptrs;
cv::Mat src;
cv::Mat dst;
cv::Mat dst2;
cv::Mat trans, trans2;
std::vector<cv::Mat> calibs;
//processing
int K = 100;
int width = 128;//56; // TODO
// pointer used in the kernels
float *src_out;
int *ids_out;
float *srcOut;
int *idsOut;
struct threshold op;
cv::Mat corners, pts3DHomo;
std::vector<std::vector<int>> face_id;
std::vector<std::vector<int>> faceId;
public:
CenternetDetection3D() {};
~CenternetDetection3D() {};
bool init(const std::string& tensor_path, const int n_classes=3, const int n_batches=1, const float conf_thresh=0.3, const std::vector<cv::Mat>& k_calibs=std::vector<cv::Mat>());
void preprocess(cv::Mat &frame, const int bi=0, const std::vector<cv::Size>& stream_size=std::vector<cv::Size>());
void preprocess(cv::Mat &frame, const int bi=0);
void postprocess(const int bi=0,const bool mAP=false);
void draw(std::vector<cv::Mat>& frames);
};
include/tkDNN/CenternetDetection3DTrack.h
+14 -16
View File
@@ -76,12 +76,12 @@ public:
std::vector<cv::Mat> inputCalibs;
std::vector<cv::Size> sz_old;
std::vector<cv::Size> szOld;
cv::Mat src;
cv::Mat dst;
cv::Mat dst2;
cv::Mat trans, trans2, trans_out;
cv::Mat trans, trans2, transOut;
/* pre inf */
bool iter0;
@@ -131,27 +131,25 @@ public:
std::vector<cv::Mat> calibs;
cv::Mat corners, pts3DHomo;
std::vector<std::vector<int>> face_id;
cv::Scalar tr_colors[256];
std::vector<std::vector<int>> faceId;
cv::Scalar trColors[256];
bool view2d = false;
//processing
struct threshold op;
float out_thresh = 0.1;
float new_thresh = 0.3;
float vis_thresh = 0.3;
float peakThreshold = 0.2;
float centerThreshold = 0.3; //default 0.5
float outThresh = 0.1;
float newThresh = 0.3;
// float peakThreshold = 0.2;
// float centerThreshold = 0.3; //default 0.5
//detections
std::vector<struct detectionRes> det_res;
int count_det;
std::vector<struct detectionRes> detRes;
int countDet;
//tracks
std::vector<std::vector<struct trackingRes>> tr_res;
std::vector<std::vector<struct trackingRes>> batchTracked;
std::vector<int> count_tr;
std::vector<int> track_id;
std::vector<std::vector<struct trackingRes>> trRes;
std::vector<int> countTr;
std::vector<int> trackId;
bool init_preprocessing();
@@ -168,7 +166,7 @@ public:
CenternetDetection3DTrack() {};
~CenternetDetection3DTrack() {};
bool init(const std::string& tensor_path, const int n_classes=3, const int n_batches=1, const float conf_thresh=0.3, const std::vector<cv::Mat>& k_calibs=std::vector<cv::Mat>());
void preprocess(cv::Mat &frame, const int bi=0, const std::vector<cv::Size>& stream_size=std::vector<cv::Size>());
void preprocess(cv::Mat &frame, const int bi=0);
void postprocess(const int bi=0,const bool mAP=false);
void draw(std::vector<cv::Mat>& frames);
};
include/tkDNN/DetectionNN3D.h
+3 -3
View File
@@ -54,7 +54,7 @@ class DetectionNN3D {
* @param frame original frame to adapt for inference.
* @param bi batch index
*/
virtual void preprocess(cv::Mat &frame, const int bi=0 , const std::vector<cv::Size>& stream_size=std::vector<cv::Size>()) = 0;
virtual void preprocess(cv::Mat &frame, const int bi=0) = 0;
/**
* This method postprocess the output of the NN to obtain the correct
@@ -102,7 +102,7 @@ class DetectionNN3D {
* box are needed, as in some cases for the mAP calculation.
*/
void update(std::vector<cv::Mat>& frames, const int cur_batches=1, bool save_times=false,
std::ofstream *times=nullptr, const bool mAP=false, const std::vector<cv::Size>& stream_size=std::vector<cv::Size>()){
std::ofstream *times=nullptr, const bool mAP=false){
if(save_times && times==nullptr)
FatalError("save_times set to true, but no valid ofstream given");
if(cur_batches > nBatches)
@@ -116,7 +116,7 @@ class DetectionNN3D {
if(!frames[bi].data)
FatalError("No image data feed to detection");
originalSize.push_back(frames[bi].size());
preprocess(frames[bi], bi, stream_size);
preprocess(frames[bi], bi);
}
TKDNN_TSTOP
pre_stats.push_back(t_ns);
src/CenternetDetection3D.cpp
+56 -51
View File
@@ -10,7 +10,7 @@ bool CenternetDetection3D::init(const std::string& tensor_path, const int n_clas
classes = n_classes;
nBatches = n_batches;
confThreshold = conf_thresh;
inputCalibs = k_calibs;
dim = netRT->input_dim;
const char *kitti_class_name[] = {
@@ -99,19 +99,26 @@ bool CenternetDetection3D::init(const std::string& tensor_path, const int n_clas
stddev << 0.229, 0.224, 0.225;
#endif
calibs = cv::Mat(cv::Size(4,3), CV_32F);
calibs.at<float>(0,0) = 707.0493;
calibs.at<float>(0,1) = 0.0;
calibs.at<float>(0,2) = 604.0814;
calibs.at<float>(0,3) = 45.75831;
calibs.at<float>(1,0) = 0.0;
calibs.at<float>(1,1) = 707.0493;
calibs.at<float>(1,2) = 180.5066;
calibs.at<float>(1,3) = -0.3454157;
calibs.at<float>(2,0) = 0.0;
calibs.at<float>(2,1) = 0.0;
calibs.at<float>(2,2) = 1.0;
calibs.at<float>(2,3) = 0.004981016;
for(int bi=0; bi<nBatches; bi++) {
cv::Mat calibs_ = cv::Mat::zeros(cv::Size(4,3), CV_32F);
if(inputCalibs.size() == 0 || inputCalibs[bi].empty()) {
calibs_.at<float>(0,0) = 707.0493;
calibs_.at<float>(0,2) = 604.0814;
calibs_.at<float>(1,1) = 707.0493;
calibs_.at<float>(1,2) = 180.5066;
}
else {
calibs_.at<float>(0,0) = inputCalibs[bi].at<float>(0,0) * dim.w / 1440;
calibs_.at<float>(0,2) = inputCalibs[bi].at<float>(0,2) * dim.w / 1440;
calibs_.at<float>(1,1) = inputCalibs[bi].at<float>(1,1) * dim.h / 1080;
calibs_.at<float>(1,2) = inputCalibs[bi].at<float>(1,2) * dim.h / 1080;
}
calibs_.at<float>(0,3) = 45.75831;
calibs_.at<float>(1,3) = -0.3454157;
calibs_.at<float>(2,2) = 1.0;
calibs_.at<float>(2,3) = 0.004981016;
calibs.push_back(calibs_);
}
r = cv::Mat(cv::Size(3,3), CV_32F);
r.at<float>(0,1) = 0.0;
@@ -139,8 +146,8 @@ bool CenternetDetection3D::init(const std::string& tensor_path, const int n_clas
checkCuda( cudaMalloc(&d_ptrs, dim.c * dim.h*dim.w * sizeof(float)) );
// Alloc array used in the kernel
checkCuda( cudaMalloc(&src_out, K *sizeof(float)) );
checkCuda( cudaMalloc(&ids_out, K *sizeof(int)) );
checkCuda( cudaMalloc(&srcOut, K *sizeof(float)) );
checkCuda( cudaMalloc(&idsOut, K *sizeof(int)) );
dst2.at<float>(0,0)=width * 0.5;
dst2.at<float>(0,1)=width * 0.5;
@@ -150,16 +157,14 @@ bool CenternetDetection3D::init(const std::string& tensor_path, const int n_clas
dst2.at<float>(2,0)=dst2.at<float>(1,0) + (-dst2.at<float>(0,1)+dst2.at<float>(1,1) );
dst2.at<float>(2,1)=dst2.at<float>(1,1) + (dst2.at<float>(0,0)-dst2.at<float>(1,0) );
face_id.push_back({0,1,5,4});
face_id.push_back({1,2,6, 5});
face_id.push_back({2,3,7,6});
face_id.push_back({3,0,4,7});
faceId.push_back({0,1,5,4});
faceId.push_back({1,2,6, 5});
faceId.push_back({2,3,7,6});
faceId.push_back({3,0,4,7});
// ([[0,1,5,4], [1,2,6, 5], [2,3,7,6], [3,0,4,7]]);
}
void CenternetDetection3D::preprocess(cv::Mat &frame, const int bi, const std::vector<cv::Size>& stream_size){
// -----------------------------------pre-process ------------------------------------------
void CenternetDetection3D::preprocess(cv::Mat &frame, const int bi){
// auto start_t = std::chrono::steady_clock::now();
// auto step_t = std::chrono::steady_clock::now();
// auto end_t = std::chrono::steady_clock::now();
@@ -338,20 +343,20 @@ void CenternetDetection3D::postprocess(const int bi, const bool mAP) {
// ----------- topk end
topKxyAddOffset(topk_inds_d, K, dim_reg.h*dim_reg.w, inttopk_xs_d, inttopk_ys_d, topk_xs_d, topk_ys_d, rt_out[3], src_out, ids_out);
topKxyAddOffset(topk_inds_d, K, dim_reg.h*dim_reg.w, inttopk_xs_d, inttopk_ys_d, topk_xs_d, topk_ys_d, rt_out[3], srcOut, idsOut);
// checkCuda( cudaDeviceSynchronize() );
getRecordsFromTopKId(topk_inds_d, K, dim_dep.c, dim_dep.h * dim_dep.w, rt_out[4], dep_d, ids_out);
getRecordsFromTopKId(topk_inds_d, K, dim_dep.c, dim_dep.h * dim_dep.w, rt_out[4], dep_d, idsOut);
checkCuda( cudaMemcpy(dep, dep_d, K * dim_dep.c * sizeof(float), cudaMemcpyDeviceToHost) );
getRecordsFromTopKId(topk_inds_d, K, dim_rot.c, dim_rot.h * dim_rot.w, rt_out[5], rot_d, ids_out);
getRecordsFromTopKId(topk_inds_d, K, dim_rot.c, dim_rot.h * dim_rot.w, rt_out[5], rot_d, idsOut);
checkCuda( cudaMemcpy(rot, rot_d, K * dim_rot.c * sizeof(float), cudaMemcpyDeviceToHost) );
getRecordsFromTopKId(topk_inds_d, K, dim_dim.c, dim_dim.h * dim_dim.w, rt_out[6], dim_d, ids_out);
getRecordsFromTopKId(topk_inds_d, K, dim_dim.c, dim_dim.h * dim_dim.w, rt_out[6], dim_d, idsOut);
checkCuda( cudaMemcpy(dim_, dim_d, K * dim_dim.c * sizeof(float), cudaMemcpyDeviceToHost) );
getRecordsFromTopKId(topk_inds_d, K, dim_wh.c, dim_wh.h * dim_wh.w, rt_out[2], wh_d, ids_out);
getRecordsFromTopKId(topk_inds_d, K, dim_wh.c, dim_wh.h * dim_wh.w, rt_out[2], wh_d, idsOut);
checkCuda( cudaMemcpy(wh, wh_d, K * dim_wh.c * sizeof(float), cudaMemcpyDeviceToHost) );
checkCuda( cudaMemcpy(xs, topk_xs_d, K * sizeof(float), cudaMemcpyDeviceToHost) );
@@ -397,11 +402,11 @@ void CenternetDetection3D::postprocess(const int bi, const bool mAP) {
alpha = std::atan2(rot[6*K + j], rot[7*K + j]) +0.5 * M_PI;
// unproject_2d_to_3d
z = dep[j] - calibs.at<float>(2,3);// z = depth - P[2, 3]
x = (target_coords[j*4] * dep[j] - calibs.at<float>(0,3) - calibs.at<float>(0,2) * z) / calibs.at<float>(0,0);
y = (target_coords[j*4+1] * dep[j] - calibs.at<float>(1,3) - calibs.at<float>(1,2) * z) / calibs.at<float>(1,1) + (dim_[j] / 2);
z = dep[j] - calibs[bi].at<float>(2,3);// z = depth - P[2, 3]
x = (target_coords[j*4] * dep[j] - calibs[bi].at<float>(0,3) - calibs[bi].at<float>(0,2) * z) / calibs[bi].at<float>(0,0);
y = (target_coords[j*4+1] * dep[j] - calibs[bi].at<float>(1,3) - calibs[bi].at<float>(1,2) * z) / calibs[bi].at<float>(1,1) + (dim_[j] / 2);
// alpha2rot_y
rot_y = (alpha + std::atan2(target_coords[j*4] - calibs.at<float>(0,2), calibs.at<float>(0,0)));
rot_y = (alpha + std::atan2(target_coords[j*4] - calibs[bi].at<float>(0,2), calibs[bi].at<float>(0,0)));
if(rot_y>M_PI)
rot_y -= 2*M_PI;
if(rot_y<M_PI)
@@ -450,7 +455,7 @@ void CenternetDetection3D::postprocess(const int bi, const bool mAP) {
pts3DHomo.at<float>(k1,k2) = aus.at<float>(k1,k2);
}
aus.release();
aus = calibs * pts3DHomo;
aus = calibs[bi] * pts3DHomo;
tk::dnn::box3D res;
for(int k=0; k<8; k++) {
@@ -486,31 +491,31 @@ void CenternetDetection3D::draw(std::vector<cv::Mat>& frames) {
for(int ind_f = 3; ind_f>=0; ind_f--) {
for(int j=0; j<4; j++) {
cv::line(frames[bi], cv::Point(b.corners.at(face_id.at(ind_f).at(j) * 2),
b.corners.at(face_id.at(ind_f).at(j) * 2 + 1)),
cv::Point(b.corners.at(face_id.at(ind_f).at((j+1)%4) * 2),
b.corners.at(face_id.at(ind_f).at((j+1)%4) * 2 + 1)),
cv::line(frames[bi], cv::Point(b.corners.at(faceId.at(ind_f).at(j) * 2),
b.corners.at(faceId.at(ind_f).at(j) * 2 + 1)),
cv::Point(b.corners.at(faceId.at(ind_f).at((j+1)%4) * 2),
b.corners.at(faceId.at(ind_f).at((j+1)%4) * 2 + 1)),
colors[b.cl], 2);
if(ind_f == 0) {
cv::line(frames[bi], cv::Point(b.corners.at(face_id.at(ind_f).at(0) * 2),
b.corners.at(face_id.at(ind_f).at(0) * 2 + 1)),
cv::Point(b.corners.at(face_id.at(ind_f).at(2) * 2),
b.corners.at(face_id.at(ind_f).at(2) * 2 + 1)), colors[b.cl], 2);
cv::line(frames[bi], cv::Point(b.corners.at(face_id.at(ind_f).at(1) * 2),
b.corners.at(face_id.at(ind_f).at(1) * 2 + 1)),
cv::Point(b.corners.at(face_id.at(ind_f).at(3) * 2),
b.corners.at(face_id.at(ind_f).at(3) * 2 + 1)), colors[b.cl], 2);
cv::line(frames[bi], cv::Point(b.corners.at(faceId.at(ind_f).at(0) * 2),
b.corners.at(faceId.at(ind_f).at(0) * 2 + 1)),
cv::Point(b.corners.at(faceId.at(ind_f).at(2) * 2),
b.corners.at(faceId.at(ind_f).at(2) * 2 + 1)), colors[b.cl], 2);
cv::line(frames[bi], cv::Point(b.corners.at(faceId.at(ind_f).at(1) * 2),
b.corners.at(faceId.at(ind_f).at(1) * 2 + 1)),
cv::Point(b.corners.at(faceId.at(ind_f).at(3) * 2),
b.corners.at(faceId.at(ind_f).at(3) * 2 + 1)), colors[b.cl], 2);
}
}
}
// draw label
cv::Size text_size = getTextSize(classesNames[b.cl], cv::FONT_HERSHEY_SIMPLEX, font_scale, thickness, &baseline);
cv::rectangle(frames[bi], cv::Point(b.corners.at(face_id.at(0).at(0) * 2),
b.corners.at(face_id.at(0).at(0) * 2 + 1)),
cv::Point((b.corners.at(face_id.at(0).at(0) * 2) + text_size.width - 2),
(b.corners.at(face_id.at(0).at(0) * 2 + 1)) - text_size.height - 2), colors[b.cl], -1);
cv::putText(frames[bi], classesNames[b.cl], cv::Point(b.corners.at(face_id.at(0).at(0) * 2),
b.corners.at(face_id.at(0).at(0) * 2 + 1) - (baseline / 2)),
cv::rectangle(frames[bi], cv::Point(b.corners.at(faceId.at(0).at(0) * 2),
b.corners.at(faceId.at(0).at(0) * 2 + 1)),
cv::Point((b.corners.at(faceId.at(0).at(0) * 2) + text_size.width - 2),
(b.corners.at(faceId.at(0).at(0) * 2 + 1)) - text_size.height - 2), colors[b.cl], -1);
cv::putText(frames[bi], classesNames[b.cl], cv::Point(b.corners.at(faceId.at(0).at(0) * 2),
b.corners.at(faceId.at(0).at(0) * 2 + 1) - (baseline / 2)),
cv::FONT_HERSHEY_SIMPLEX, font_scale, cv::Scalar(255, 255, 255), thickness);
}
}
src/CenternetDetection3DTrack.cpp
+315 -312
View File
@@ -6,83 +6,77 @@ namespace tk { namespace dnn {
bool CenternetDetection3DTrack::init(const std::string& tensor_path, const int n_classes, const int n_batches,
const float conf_thresh, const std::vector<cv::Mat>& k_calibs) {
netRT = new tk::dnn::NetworkRT(NULL, (tensor_path).c_str() );
dim = netRT->input_dim;
dim.c = 3;
nBatches = n_batches;
netRT = new tk::dnn::NetworkRT(NULL, (tensor_path).c_str() );
dim = netRT->input_dim;
dim.c = 3;
nBatches = n_batches;
confThreshold = conf_thresh;
inputCalibs = k_calibs;
tr_res.resize(nBatches);
count_tr.resize(nBatches, 0);
inputCalibs = k_calibs;
init_preprocessing();
init_pre_inf();
init_postprocessing();
init_visualization(n_classes);
}
bool CenternetDetection3DTrack::init_preprocessing(){
//image transformation
src = cv::Mat(cv::Size(2,3), CV_32F);
dst = cv::Mat(cv::Size(2,3), CV_32F);
dst2 = cv::Mat(cv::Size(2,3), CV_32F);
trans = cv::Mat(cv::Size(3,2), CV_32F);
trans2 = cv::Mat(cv::Size(3,2), CV_32F);
trans_out = cv::Mat(cv::Size(3,2), CV_32F);
src = cv::Mat(cv::Size(2,3), CV_32F);
dst = cv::Mat(cv::Size(2,3), CV_32F);
dst2 = cv::Mat(cv::Size(2,3), CV_32F);
trans = cv::Mat(cv::Size(3,2), CV_32F);
trans2 = cv::Mat(cv::Size(3,2), CV_32F);
transOut = cv::Mat(cv::Size(3,2), CV_32F);
dst2.at<float>(0,0)=width * 0.5;
dst2.at<float>(0,1)=width * 0.5;
dst2.at<float>(1,0)=width * 0.5;
dst2.at<float>(1,1)=width * 0.5 + width * -0.5;
dst2.at<float>(2,0)=dst2.at<float>(1,0) + (-dst2.at<float>(0,1)+dst2.at<float>(1,1) );
dst2.at<float>(2,1)=dst2.at<float>(1,1) + (dst2.at<float>(0,0)-dst2.at<float>(1,0) );
dst2.at<float>(0,0) = width * 0.5;
dst2.at<float>(0,1) = width * 0.5;
dst2.at<float>(1,0) = width * 0.5;
dst2.at<float>(1,1) = width * 0.5 + width * -0.5;
dst2.at<float>(2,0) = dst2.at<float>(1,0) + (-dst2.at<float>(0,1)+dst2.at<float>(1,1) );
dst2.at<float>(2,1) = dst2.at<float>(1,1) + (dst2.at<float>(0,0)-dst2.at<float>(1,0) );
for(int bi=0; bi<nBatches; bi++) {
sz_old.push_back(cv::Size(0,0));
szOld.push_back(cv::Size(0,0));
}
#ifdef OPENCV_CUDACONTRIB
std::cout<<"OPENCV CPMTROB\n";
checkCuda( cudaMalloc(&mean_d, 3 * sizeof(float)) );
checkCuda( cudaMalloc(&stddev_d, 3 * sizeof(float)) );
float mean[3] = {0.40789655, 0.44719303, 0.47026116};
float mean[3] = {0.40789655, 0.44719303, 0.47026116};
float stddev[3] = {0.2886383, 0.27408165, 0.27809834};
checkCuda(cudaMemcpy(mean_d, mean, 3*sizeof(float), cudaMemcpyHostToDevice));
checkCuda(cudaMemcpy(stddev_d, stddev, 3*sizeof(float), cudaMemcpyHostToDevice));
checkCuda( cudaMemcpy(mean_d, mean, 3*sizeof(float), cudaMemcpyHostToDevice));
checkCuda( cudaMemcpy(stddev_d, stddev, 3*sizeof(float), cudaMemcpyHostToDevice));
#else
checkCuda(cudaMallocHost(&input, sizeof(dnnType)*dim.tot() * nBatches));
mean << 0.40789655, 0.44719303, 0.47026116;
stddev << 0.2886383, 0.27408165, 0.27809834;
std::cout<<"NO OPENCV CPMTROB\n";
checkCuda( cudaMallocHost(&input, sizeof(dnnType)*dim.tot() * nBatches));
mean << 0.40789655, 0.44719303, 0.47026116;
stddev << 0.2886383, 0.27408165, 0.27809834;
#endif
checkCuda(cudaMalloc(&input_d, sizeof(dnnType)*netRT->input_dim.tot() * nBatches));
checkCuda(cudaMalloc(&input_pre_inf_d, sizeof(dnnType)*dim.tot()));
checkCuda( cudaMalloc(&input_d, sizeof(dnnType)*netRT->input_dim.tot() * nBatches));
checkCuda( cudaMalloc(&input_pre_inf_d, sizeof(dnnType)*dim.tot()));
checkCuda( cudaMalloc(&d_ptrs, dim.tot() * sizeof(float)) );
}
bool CenternetDetection3DTrack::init_pre_inf(){
// initial steps: the first part of the network
const char *pre_img_conv1_bin = "dla34_cnet3d_track/layers/base-pre_img_layer-0.bin";
const char *pre_hm_conv1_bin = "dla34_cnet3d_track/layers/base-pre_hm_layer-0.bin";
const char *conv1_bin = "dla34_cnet3d_track/layers/base-base_layer-0.bin";
const char *conv2_bin = "dla34_cnet3d_track/layers/base-level0-0.bin";
const char *pre_hm_conv1_bin = "dla34_cnet3d_track/layers/base-pre_hm_layer-0.bin";
const char *conv1_bin = "dla34_cnet3d_track/layers/base-base_layer-0.bin";
const char *conv2_bin = "dla34_cnet3d_track/layers/base-level0-0.bin";
dim_in0 = tk::dnn::dataDim_t(1, 3, 512, 512, 1);
dim_in1 = tk::dnn::dataDim_t(1, 1, 512, 512, 1);
checkCuda( cudaMalloc(&out_d, netRT->input_dim.tot()*sizeof(dnnType)) );
checkCuda( cudaMalloc(&img_d, dim_in0.tot()*sizeof(dnnType)) );
checkCuda( cudaMalloc(&hm_d, dim_in1.tot()*sizeof(dnnType)) );
// init to zeros hm
dnnType *hm_h;
dnnType *hm_h;
checkCuda( cudaMallocHost(&hm_h, 1 * dim.h * dim.w*sizeof(dnnType)) );
for(int i=0; i<1 * dim.h * dim.w; i++)
hm_h[i]=0.0f;
hm_h[i] = 0.0f;
checkCuda( cudaMemcpy(hm_d, hm_h, 1 * dim.h * dim.w * sizeof(dnnType), cudaMemcpyHostToDevice) );
checkCuda( cudaFreeHost(hm_h) );
dnnType *i0_h, *i1_h, *i2_h;
@@ -97,20 +91,20 @@ bool CenternetDetection3DTrack::init_pre_inf(){
pre_phase_net = new tk::dnn::Network(dim_in0);
//pre-img
tk::dnn::Input *in_pre_img = new tk::dnn::Input(pre_phase_net, dim_in0, img_d);
tk::dnn::Conv2d *pre_img_conv1 = new tk::dnn::Conv2d(pre_phase_net, 16, 7, 7, 1, 1, 3, 3, pre_img_conv1_bin, true);
tk::dnn::Activation *pre_img_relu = new tk::dnn::Activation(pre_phase_net, CUDNN_ACTIVATION_RELU);
tk::dnn::Input *in_pre_img = new tk::dnn::Input(pre_phase_net, dim_in0, img_d);
tk::dnn::Conv2d *pre_img_conv1 = new tk::dnn::Conv2d(pre_phase_net, 16, 7, 7, 1, 1, 3, 3, pre_img_conv1_bin, true);
tk::dnn::Activation *pre_img_relu = new tk::dnn::Activation(pre_phase_net, CUDNN_ACTIVATION_RELU);
//pre-hm
tk::dnn::Input *in_pre_hm = new tk::dnn::Input(pre_phase_net, dim_in1, hm_d);
tk::dnn::Conv2d *pre_hm_conv1 = new tk::dnn::Conv2d(pre_phase_net, 16, 7, 7, 1, 1, 3, 3, pre_hm_conv1_bin, true);
tk::dnn::Activation *pre_hm_relu = new tk::dnn::Activation(pre_phase_net, CUDNN_ACTIVATION_RELU);
tk::dnn::Input *in_pre_hm = new tk::dnn::Input(pre_phase_net, dim_in1, hm_d);
tk::dnn::Conv2d *pre_hm_conv1 = new tk::dnn::Conv2d(pre_phase_net, 16, 7, 7, 1, 1, 3, 3, pre_hm_conv1_bin, true);
tk::dnn::Activation *pre_hm_relu = new tk::dnn::Activation(pre_phase_net, CUDNN_ACTIVATION_RELU);
// image input
tk::dnn::Input *input_image = new tk::dnn::Input(pre_phase_net, dim_in0, input_pre_inf_d);
tk::dnn::Conv2d *conv1 = new tk::dnn::Conv2d(pre_phase_net, 16, 7, 7, 1, 1, 3, 3, conv1_bin, true);
tk::dnn::Activation *relu1 = new tk::dnn::Activation(pre_phase_net, CUDNN_ACTIVATION_RELU);
tk::dnn::Input *input_image = new tk::dnn::Input(pre_phase_net, dim_in0, input_pre_inf_d);
tk::dnn::Conv2d *conv1 = new tk::dnn::Conv2d(pre_phase_net, 16, 7, 7, 1, 1, 3, 3, conv1_bin, true);
tk::dnn::Activation *relu1 = new tk::dnn::Activation(pre_phase_net, CUDNN_ACTIVATION_RELU);
tk::dnn::Shortcut *s0_input = new tk::dnn::Shortcut(pre_phase_net, pre_img_relu);
tk::dnn::Shortcut *s1_input = new tk::dnn::Shortcut(pre_phase_net, pre_hm_relu);
tk::dnn::Shortcut *s0_input = new tk::dnn::Shortcut(pre_phase_net, pre_img_relu);
tk::dnn::Shortcut *s1_input = new tk::dnn::Shortcut(pre_phase_net, pre_hm_relu);
// output data
out_d = s1_input->dstData;
//print network model
@@ -123,14 +117,14 @@ bool CenternetDetection3DTrack::init_pre_inf(){
bool CenternetDetection3DTrack::init_postprocessing(){
srand(0); //seed = 0 for random colors
dim_hm = tk::dnn::dataDim_t(1, 10, 128, 128, 1);
dim_wh = tk::dnn::dataDim_t(1, 2, 128, 128, 1);
dim_reg = tk::dnn::dataDim_t(1, 2, 128, 128, 1);
dim_track = tk::dnn::dataDim_t(1, 2, 128, 128, 1);
dim_dep = tk::dnn::dataDim_t(1, 1, 128, 128, 1);
dim_rot = tk::dnn::dataDim_t(1, 8, 128, 128, 1);
dim_dim = tk::dnn::dataDim_t(1, 3, 128, 128, 1);
dim_amodel_offset = tk::dnn::dataDim_t(1, 2, 128, 128, 1);
dim_hm = tk::dnn::dataDim_t(1, 10, 128, 128, 1);
dim_wh = tk::dnn::dataDim_t(1, 2, 128, 128, 1);
dim_reg = tk::dnn::dataDim_t(1, 2, 128, 128, 1);
dim_track = tk::dnn::dataDim_t(1, 2, 128, 128, 1);
dim_dep = tk::dnn::dataDim_t(1, 1, 128, 128, 1);
dim_rot = tk::dnn::dataDim_t(1, 8, 128, 128, 1);
dim_dim = tk::dnn::dataDim_t(1, 3, 128, 128, 1);
dim_amodel_offset = tk::dnn::dataDim_t(1, 2, 128, 128, 1);
checkCuda( cudaMalloc(&topk_scores, dim_hm.c * K *sizeof(float)) );
checkCuda( cudaMalloc(&topk_inds_, dim_hm.c * K *sizeof(int)) );
@@ -138,7 +132,7 @@ bool CenternetDetection3DTrack::init_postprocessing(){
checkCuda( cudaMalloc(&topk_xs_, dim_hm.c * K *sizeof(float)) );
checkCuda( cudaMalloc(&ids_d, dim_hm.c * dim_hm.h * dim_hm.w*sizeof(int)) );
checkCuda( cudaMallocHost(&ids_, dim_hm.c * dim_hm.h * dim_hm.w*sizeof(int)) );
for(int i =0; i<dim_hm.c * dim_hm.h * dim_hm.w; i++){
for(int i=0; i<dim_hm.c * dim_hm.h * dim_hm.w; i++){
ids_[i] = i;
}
@@ -146,7 +140,7 @@ bool CenternetDetection3DTrack::init_postprocessing(){
float *ones_h;
checkCuda( cudaMallocHost(&ones_h, dim_dep.c * dim_dep.h * dim_dep.w * sizeof(float)) );
for(int i=0; i<dim_dep.c * dim_dep.h * dim_dep.w; i++)
ones_h[i]=1.0f;
ones_h[i] = 1.0f;
checkCuda( cudaMemcpy(ones, ones_h, dim_dep.c * dim_dep.h * dim_dep.w * sizeof(float), cudaMemcpyHostToDevice) );
checkCuda( cudaFreeHost(ones_h) );
@@ -204,10 +198,9 @@ bool CenternetDetection3DTrack::init_postprocessing(){
checkCuda( cudaMalloc(&src_out, K *sizeof(float)) );
checkCuda( cudaMalloc(&ids_out, K *sizeof(int)) );
for(int bi=0; bi<nBatches; bi++){
track_id.push_back(0);
count_tr.push_back(0);
}
trRes.resize(nBatches);
countTr.resize(nBatches, 0);
trackId.resize(nBatches, 0);
}
bool CenternetDetection3DTrack::init_visualization(const int n_classes){
@@ -238,18 +231,18 @@ bool CenternetDetection3DTrack::init_visualization(const int n_classes){
// classesNames = std::vector<std::string>(coco_class_name, std::end( coco_class_name));
for(int c=0; c<classes; c++) {
int offset = c*123457 % classes;
float r = getColor(2, offset, classes);
float g = getColor(1, offset, classes);
float b = getColor(0, offset, classes);
colors[c] = cv::Scalar(int(255.0*b), int(255.0*g), int(255.0*r));
int offset = c*123457 % classes;
float r = getColor(2, offset, classes);
float g = getColor(1, offset, classes);
float b = getColor(0, offset, classes);
colors[c] = cv::Scalar(int(255.0*b), int(255.0*g), int(255.0*r));
}
for(int c=0; c<256; c++) {
int offset = c*123457 % 256;
float r = getColor(2, offset, 256);
float g = getColor(1, offset, 256);
float b = getColor(0, offset, 256);
tr_colors[c] = cv::Scalar(int(255.0*b), int(255.0*g), int(255.0*r));
int offset = c * 123457 % 256;
float r = getColor(2, offset, 256);
float g = getColor(1, offset, 256);
float b = getColor(0, offset, 256);
trColors[c] = cv::Scalar(int(255.0*b), int(255.0*g), int(255.0*r));
}
r = cv::Mat(cv::Size(3,3), CV_32F);
@@ -275,10 +268,10 @@ bool CenternetDetection3DTrack::init_visualization(const int n_classes){
pts3DHomo.at<float>(3,6) = 1.0;
pts3DHomo.at<float>(3,7) = 1.0;
face_id.push_back({0,1,5,4});
face_id.push_back({1,2,6, 5});
face_id.push_back({3,0,4,7});
face_id.push_back({2,3,7,6});
faceId.push_back({0,1,5,4});
faceId.push_back({1,2,6, 5});
faceId.push_back({3,0,4,7});
faceId.push_back({2,3,7,6});
// ([[0,1,5,4], [1,2,6, 5], [2,3,7,6], [3,0,4,7]]);
}
@@ -296,22 +289,21 @@ void CenternetDetection3DTrack::pre_inf(const int bi){
checkCuda( cudaDeviceSynchronize() );
}
void CenternetDetection3DTrack::preprocess(cv::Mat &frame, const int bi, const std::vector<cv::Size>& stream_size){
// -----------------------------------pre-process ------------------------------------------
void CenternetDetection3DTrack::preprocess(cv::Mat &frame, const int bi){
cv::Size sz = originalSize[bi];
float scale = 1.0;
float new_height = sz.height * scale;
float new_width = sz.width * scale;
if(sz.height != sz_old[bi].height && sz.width != sz_old[bi].width){
// float scale = 1.0;
float new_height = dim.h;//sz.height * scale;
float new_width = dim.w;//sz.width * scale;
if(sz.height != szOld[bi].height && sz.width != szOld[bi].width){
if(inputCalibs.size() == 0 || inputCalibs[bi].empty()) {
calibs[bi].at<float>(0,2) = new_width / 2.0f;
calibs[bi].at<float>(1,2) = new_height /2.0f;
}
else {
calibs[bi].at<float>(0,0) = inputCalibs[bi].at<float>(0,0) * dim.w / stream_size[bi].width;
calibs[bi].at<float>(0,2) = inputCalibs[bi].at<float>(0,2) * dim.w / stream_size[bi].width;
calibs[bi].at<float>(1,1) = inputCalibs[bi].at<float>(1,1) * dim.h / stream_size[bi].height;
calibs[bi].at<float>(1,2) = inputCalibs[bi].at<float>(1,2) * dim.h / stream_size[bi].height;
calibs[bi].at<float>(0,0) = inputCalibs[bi].at<float>(0,0) * dim.w / sz.width;
calibs[bi].at<float>(0,2) = inputCalibs[bi].at<float>(0,2) * dim.w / sz.width;
calibs[bi].at<float>(1,1) = inputCalibs[bi].at<float>(1,1) * dim.h / sz.height;
calibs[bi].at<float>(1,2) = inputCalibs[bi].at<float>(1,2) * dim.h / sz.height;
}
float c[] = {new_width / 2.0f, new_height /2.0f};
@@ -320,34 +312,33 @@ void CenternetDetection3DTrack::preprocess(cv::Mat &frame, const int bi, const s
// ----------- get_affine_transform
// rot_rad = pi * 0 / 100 --> 0
//dim.print();
src.at<float>(0,0)=c[0];
src.at<float>(0,1)=c[1];
src.at<float>(1,0)=c[0];
src.at<float>(1,1)=c[1] + s[0] * -0.5;
dst.at<float>(0,0)=dim.w * 0.5;
dst.at<float>(0,1)=dim.h * 0.5;
dst.at<float>(1,0)=dim.w * 0.5;
dst.at<float>(1,1)=dim.h * 0.5 + dim.w * -0.5;
src.at<float>(0,0) = c[0];
src.at<float>(0,1) = c[1];
src.at<float>(1,0) = c[0];
src.at<float>(1,1) = c[1] + s[0] * -0.5;
dst.at<float>(0,0) = dim.w * 0.5;
dst.at<float>(0,1) = dim.h * 0.5;
dst.at<float>(1,0) = dim.w * 0.5;
dst.at<float>(1,1) = dim.h * 0.5 + dim.w * -0.5;
src.at<float>(2,0)=src.at<float>(1,0) + (-src.at<float>(0,1)+src.at<float>(1,1) );
src.at<float>(2,1)=src.at<float>(1,1) + (src.at<float>(0,0)-src.at<float>(1,0) );
dst.at<float>(2,0)=dst.at<float>(1,0) + (-dst.at<float>(0,1)+dst.at<float>(1,1) );
dst.at<float>(2,1)=dst.at<float>(1,1) + (dst.at<float>(0,0)-dst.at<float>(1,0) );
src.at<float>(2,0) = src.at<float>(1,0) + (-src.at<float>(0,1)+src.at<float>(1,1) );
src.at<float>(2,1) = src.at<float>(1,1) + (src.at<float>(0,0)-src.at<float>(1,0) );
dst.at<float>(2,0) = dst.at<float>(1,0) + (-dst.at<float>(0,1)+dst.at<float>(1,1) );
dst.at<float>(2,1) = dst.at<float>(1,1) + (dst.at<float>(0,0)-dst.at<float>(1,0) );
trans = cv::getAffineTransform( src, dst );
trans2 = cv::getAffineTransform( dst2, src );
trans2.convertTo(trans_out, CV_32F);
trans2.convertTo(transOut, CV_32F);
}
sz_old[bi] = sz;
szOld[bi] = sz;
#ifdef OPENCV_CUDACONTRIB
std::cout<<"OPENCV CPMTROB\n";
cv::cuda::GpuMat im_Orig;
cv::cuda::GpuMat imageF1_d, imageF2_d;
im_Orig = cv::cuda::GpuMat(frame);
// cv::cuda::resize (im_Orig, imageF1_d, cv::Size(new_width, new_height));
imageF1_d = im_Orig;
cv::cuda::resize (im_Orig, imageF1_d, cv::Size(dim.w, dim.h));
// imageF1_d = im_Orig;
checkCuda( cudaDeviceSynchronize() );
sz = imageF1_d.size();
@@ -367,20 +358,18 @@ void CenternetDetection3DTrack::preprocess(cv::Mat &frame, const int bi, const s
normalize(d_ptrs, dim.c, dim.h, dim.w, mean_d, stddev_d);
checkCuda(cudaMemcpy(input_pre_inf_d, d_ptrs, dim2.tot()*sizeof(dnnType), cudaMemcpyDeviceToDevice));
checkCuda( cudaMemcpy(input_pre_inf_d, d_ptrs, dim2.tot()*sizeof(dnnType), cudaMemcpyDeviceToDevice));
checkCuda( cudaDeviceSynchronize() );
#else
std::cout<<"NO OPENCV CPMTROB\n";
cv::Mat imageF;
//resize(frame, imageF, cv::Size(512, 512));
imageF = frame;
resize(frame, imageF, cv::Size(dim.w, dim.h));
// imageF = frame;
sz = imageF.size();
cv::warpAffine(imageF, imageF, trans, cv::Size(dim.w, dim.h), cv::INTER_LINEAR );
//cv::imshow("warp", imageF);
// cv::imshow("warp", imageF);
sz = imageF.size();
imageF.convertTo(imageF, CV_32FC3, 1/255.0);
@@ -394,11 +383,11 @@ void CenternetDetection3DTrack::preprocess(cv::Mat &frame, const int bi, const s
bgr[i] = bgr[i] / stddev[i];
}
for(int i=0; i<dim2.c; i++) {
int idx = i*imageF.rows*imageF.cols;
int ch =i;//dim2.c-3 +i;//i;//
int idx = i * imageF.rows * imageF.cols;
int ch = i;
memcpy((void*)&input[idx], (void*)bgr[ch].data, imageF.rows*imageF.cols*sizeof(dnnType));
}
checkCuda(cudaMemcpyAsync(input_pre_inf_d, input, dim2.tot()*sizeof(dnnType), cudaMemcpyHostToDevice));
checkCuda( cudaMemcpyAsync(input_pre_inf_d, input, dim2.tot()*sizeof(dnnType), cudaMemcpyHostToDevice));
checkCuda( cudaDeviceSynchronize() );
#endif
@@ -419,195 +408,195 @@ cv::Mat CenternetDetection3DTrack::transform_preds_with_trans(float x1, float x2
target_coords.at<float>(0,0) = x1;
target_coords.at<float>(0,1) = x2;
target_coords.at<float>(0,2) = 1.0;
return trans_out * target_coords;
return transOut * target_coords;
}
void CenternetDetection3DTrack::tracking(const int bi) {
float item_size[count_det];
int item_cl[count_det];
float dets[2*count_det];
for(int i=0; i<count_det; i++){
item_size[i] = (det_res[i].bb1.at<float>(0,0) - det_res[i].bb0.at<float>(0,0)) *
(det_res[i].bb1.at<float>(0,1) - det_res[i].bb0.at<float>(0,1));
item_cl[i] = det_res[i].cl;
dets[i*2] = det_res[i].ct.at<float>(0,0);
dets[i*2+1] = det_res[i].ct.at<float>(0,1);
float item_size[countDet];
int item_cl[countDet];
float dets[2*countDet];
for(int i=0; i<countDet; i++){
item_size[i] = (detRes[i].bb1.at<float>(0,0) - detRes[i].bb0.at<float>(0,0)) *
(detRes[i].bb1.at<float>(0,1) - detRes[i].bb0.at<float>(0,1));
item_cl[i] = detRes[i].cl;
dets[i*2] = detRes[i].ct.at<float>(0,0);
dets[i*2+1] = detRes[i].ct.at<float>(0,1);
}
float track_size[count_tr[bi]];
int track_cl[count_tr[bi]];
float tracks[2*count_tr[bi]];
for(int i=0; i<count_tr[bi]; i++){
track_size[i] = (tr_res[bi][i].det_res.bb1.at<float>(0,0) - tr_res[bi][i].det_res.bb0.at<float>(0,0)) *
(tr_res[bi][i].det_res.bb1.at<float>(0,1) - tr_res[bi][i].det_res.bb0.at<float>(0,1));
track_cl[i] = tr_res[bi][i].det_res.cl;
tracks[i*2] = tr_res[bi][i].det_res.ct.at<float>(0,0);
tracks[i*2+1] = tr_res[bi][i].det_res.ct.at<float>(0,1);
float track_size[countTr[bi]];
int track_cl[countTr[bi]];
float tracks[2*countTr[bi]];
for(int i=0; i<countTr[bi]; i++){
track_size[i] = (trRes[bi][i].det_res.bb1.at<float>(0,0) - trRes[bi][i].det_res.bb0.at<float>(0,0)) *
(trRes[bi][i].det_res.bb1.at<float>(0,1) - trRes[bi][i].det_res.bb0.at<float>(0,1));
track_cl[i] = trRes[bi][i].det_res.cl;
tracks[i*2] = trRes[bi][i].det_res.ct.at<float>(0,0);
tracks[i*2+1] = trRes[bi][i].det_res.ct.at<float>(0,1);
}
float dist[count_tr[bi]*count_det];
float dist[countTr[bi]*countDet];
bool invalid;
for(int i=0; i<count_tr[bi]; i++){
for(int j=0; j<count_det; j++){
dist[j*count_tr[bi]+i] = pow((tracks[i*2] - dets[j*2]), 2) +
pow((tracks[i*2+1] - dets[j*2+1]), 2);
invalid = dist[j*count_tr[bi]+i] > track_size[i] || dist[j*count_tr[bi]+i] > item_size[j] || item_cl[j] != track_cl[i];
dist[j*count_tr[bi]+i] = dist[j*count_tr[bi]+i] + invalid * (1 << 18);
for(int i=0; i<countTr[bi]; i++){
for(int j=0; j<countDet; j++){
dist[j*countTr[bi]+i] = pow((tracks[i*2] - dets[j*2]), 2) +
pow((tracks[i*2+1] - dets[j*2+1]), 2);
invalid = dist[j*countTr[bi]+i] > track_size[i] ||
dist[j*countTr[bi]+i] > item_size[j] ||
item_cl[j] != track_cl[i];
dist[j*countTr[bi]+i] = dist[j*countTr[bi]+i] + invalid * (1 << 18);
}
}
int matched_indices[2*count_tr[bi]];
int matched_indices[2*countTr[bi]];
float min_tr;
int min_idtr=-1;
for(int i=0; i<count_tr[bi]; i++) {
matched_indices[i*2] = -1;
matched_indices[i*2+1] = -1;
int min_idtr = -1;
for(int i=0; i<countTr[bi]; i++) {
matched_indices[i*2] = -1;
matched_indices[i*2+1] = -1;
}
for(int i=0; i<count_det; i++){
for(int i=0; i<countDet; i++){
min_tr=(1 << 18);
for(int j=0; j<count_tr[bi]; j++){
if(dist[i*count_tr[bi]+j]<min_tr) {
min_tr = dist[i*count_tr[bi]+j];
for(int j=0; j<countTr[bi]; j++){
if(dist[i*countTr[bi]+j]<min_tr) {
min_tr = dist[i*countTr[bi]+j];
min_idtr = j;
}
}
if(min_tr < (1<<16)) {
for(int j=0; j<count_det; j++){
dist[j*count_tr[bi]+min_idtr] = (1 << 18);
}
matched_indices[2*min_idtr] = min_idtr;
for(int j=0; j<countDet; j++)
dist[j*countTr[bi]+min_idtr] = (1 << 18);
matched_indices[2*min_idtr] = min_idtr;
matched_indices[2*min_idtr+1] = i;
}
}
bool unmatched_dets[count_det];
for(int i=0; i<count_det; i++)
bool unmatched_dets[countDet];
for(int i=0; i<countDet; i++)
unmatched_dets[i] = false;
bool unmatched_tracks[count_tr[bi]];
for(int i=0; i<count_tr[bi]; i++)
bool unmatched_tracks[countTr[bi]];
for(int i=0; i<countTr[bi]; i++)
unmatched_tracks[i] = false;
for(int i=0; i<count_tr[bi]; i++) {
for(int i=0; i<countTr[bi]; i++) {
if(matched_indices[2*i] != -1)
unmatched_tracks[matched_indices[2*i]]=true;
if(matched_indices[2*i+1] != -1)
unmatched_dets[matched_indices[2*i+1]]=true;
}
//match
for(int i=0; i<count_tr[bi]; i++) {
for(int i=0; i<countTr[bi]; i++) {
if(matched_indices[2*i+1] != -1 && matched_indices[2*i] != -1) { //second condition is optional
int tr_id = matched_indices[2*i];
int d_id = matched_indices[2*i+1];
int d_id = matched_indices[2*i+1];
// tr_res[tr_id].det_res = det_res[d_id];
tr_res[bi][tr_id].det_res.score = det_res[d_id].score;
tr_res[bi][tr_id].det_res.cl = det_res[d_id].cl;
tr_res[bi][tr_id].det_res.ct = det_res[d_id].ct;
tr_res[bi][tr_id].det_res.tr = det_res[d_id].tr;
tr_res[bi][tr_id].det_res.bb0 = det_res[d_id].bb0;
tr_res[bi][tr_id].det_res.bb1 = det_res[d_id].bb1;
tr_res[bi][tr_id].det_res.dep = det_res[d_id].dep;
tr_res[bi][tr_id].det_res.dim[0] = det_res[d_id].dim[0];
tr_res[bi][tr_id].det_res.dim[1] = det_res[d_id].dim[1];
tr_res[bi][tr_id].det_res.dim[2] = det_res[d_id].dim[2];
tr_res[bi][tr_id].det_res.alpha = det_res[d_id].alpha;
tr_res[bi][tr_id].det_res.x = det_res[d_id].x;
tr_res[bi][tr_id].det_res.y = det_res[d_id].y;
tr_res[bi][tr_id].det_res.z = det_res[d_id].z;
tr_res[bi][tr_id].det_res.rot_y = det_res[d_id].rot_y;
// tr_res[bi][matched_indices[2*i]].tracking_id = ; is the same
// tr_res[bi][matched_indices[2*i]].color = ; is the same
tr_res[bi][tr_id].age = 1;
tr_res[bi][tr_id].active = tr_res[bi][tr_id].active+1;
// trRes[tr_id].det_res = detRes[d_id];
trRes[bi][tr_id].det_res.score = detRes[d_id].score;
trRes[bi][tr_id].det_res.cl = detRes[d_id].cl;
trRes[bi][tr_id].det_res.ct = detRes[d_id].ct;
trRes[bi][tr_id].det_res.tr = detRes[d_id].tr;
trRes[bi][tr_id].det_res.bb0 = detRes[d_id].bb0;
trRes[bi][tr_id].det_res.bb1 = detRes[d_id].bb1;
trRes[bi][tr_id].det_res.dep = detRes[d_id].dep;
trRes[bi][tr_id].det_res.dim[0] = detRes[d_id].dim[0];
trRes[bi][tr_id].det_res.dim[1] = detRes[d_id].dim[1];
trRes[bi][tr_id].det_res.dim[2] = detRes[d_id].dim[2];
trRes[bi][tr_id].det_res.alpha = detRes[d_id].alpha;
trRes[bi][tr_id].det_res.x = detRes[d_id].x;
trRes[bi][tr_id].det_res.y = detRes[d_id].y;
trRes[bi][tr_id].det_res.z = detRes[d_id].z;
trRes[bi][tr_id].det_res.rot_y = detRes[d_id].rot_y;
// trRes[bi][matched_indices[2*i]].tracking_id = ; is the same
// trRes[bi][matched_indices[2*i]].color = ; is the same
trRes[bi][tr_id].age = 1;
trRes[bi][tr_id].active = trRes[bi][tr_id].active+1;
}
}
//delete target umatched track
int new_count_tr = 0;
for(int i=0; i<count_tr[bi]; i++) {
for(int i=0; i<countTr[bi]; i++) {
if(unmatched_tracks[i])
new_count_tr++;
}
if(new_count_tr == 0 && count_tr[bi] != 0) { //reset
tr_res[bi].clear();
count_tr[bi] = 0;
if(new_count_tr == 0 && countTr[bi] != 0) { //reset
trRes[bi].clear();
countTr[bi] = 0;
}
int old_count_tr = count_tr[bi];
if(count_tr[bi] != 0 && new_count_tr != count_tr[bi]) {
int old_count_tr = countTr[bi];
if(countTr[bi] != 0 && new_count_tr != countTr[bi]) {
std::vector<struct trackingRes> new_tr_res;
int id_new_tr=0;
for(int i=0; i<count_tr[bi]; i++) {
for(int i=0; i<countTr[bi]; i++) {
if(unmatched_tracks[i]) {
struct trackingRes new_tr_res_;
// new_tr_res_new_det_res.det_res = tr_res[i].det_res;
new_tr_res_.det_res.score = tr_res[bi][i].det_res.score;
new_tr_res_.det_res.cl = tr_res[bi][i].det_res.cl;
new_tr_res_.det_res.ct = tr_res[bi][i].det_res.ct;
new_tr_res_.det_res.tr = tr_res[bi][i].det_res.tr;
new_tr_res_.det_res.bb0 = tr_res[bi][i].det_res.bb0;
new_tr_res_.det_res.bb1 = tr_res[bi][i].det_res.bb1;
new_tr_res_.det_res.dep = tr_res[bi][i].det_res.dep;
new_tr_res_.det_res.dim[0] = tr_res[bi][i].det_res.dim[0];
new_tr_res_.det_res.dim[1] = tr_res[bi][i].det_res.dim[1];
new_tr_res_.det_res.dim[2] = tr_res[bi][i].det_res.dim[2];
new_tr_res_.det_res.alpha = tr_res[bi][i].det_res.alpha;
new_tr_res_.det_res.x = tr_res[bi][i].det_res.x;
new_tr_res_.det_res.y = tr_res[bi][i].det_res.y;
new_tr_res_.det_res.z = tr_res[bi][i].det_res.z;
new_tr_res_.det_res.rot_y = tr_res[bi][i].det_res.rot_y;
new_tr_res_.tracking_id = tr_res[bi][i].tracking_id;
new_tr_res_.age = tr_res[bi][i].age;
new_tr_res_.active = tr_res[bi][i].active;
new_tr_res_.color = tr_res[bi][i].color;
id_new_tr++;
// new_tr_res_new_det_res.det_res = trRes[i].det_res;
new_tr_res_.det_res.score = trRes[bi][i].det_res.score;
new_tr_res_.det_res.cl = trRes[bi][i].det_res.cl;
new_tr_res_.det_res.ct = trRes[bi][i].det_res.ct;
new_tr_res_.det_res.tr = trRes[bi][i].det_res.tr;
new_tr_res_.det_res.bb0 = trRes[bi][i].det_res.bb0;
new_tr_res_.det_res.bb1 = trRes[bi][i].det_res.bb1;
new_tr_res_.det_res.dep = trRes[bi][i].det_res.dep;
new_tr_res_.det_res.dim[0] = trRes[bi][i].det_res.dim[0];
new_tr_res_.det_res.dim[1] = trRes[bi][i].det_res.dim[1];
new_tr_res_.det_res.dim[2] = trRes[bi][i].det_res.dim[2];
new_tr_res_.det_res.alpha = trRes[bi][i].det_res.alpha;
new_tr_res_.det_res.x = trRes[bi][i].det_res.x;
new_tr_res_.det_res.y = trRes[bi][i].det_res.y;
new_tr_res_.det_res.z = trRes[bi][i].det_res.z;
new_tr_res_.det_res.rot_y = trRes[bi][i].det_res.rot_y;
new_tr_res_.tracking_id = trRes[bi][i].tracking_id;
new_tr_res_.age = trRes[bi][i].age;
new_tr_res_.active = trRes[bi][i].active;
new_tr_res_.color = trRes[bi][i].color;
id_new_tr ++;
new_tr_res.push_back(new_tr_res_);
}
}
if(count_tr[bi]) {
tr_res[bi].clear();
if(countTr[bi]) {
trRes[bi].clear();
}
count_tr[bi] = new_count_tr;
tr_res[bi]=new_tr_res;
countTr[bi] = new_count_tr;
trRes[bi] = new_tr_res;
}
int count_tr_ = count_tr[bi];
for(int i=0; i<count_det; i++) {
if((!unmatched_dets[i]) && det_res[i].score > new_thresh) {
int count_tr_ = countTr[bi];
for(int i=0; i<countDet; i++) {
if((!unmatched_dets[i]) && detRes[i].score > newThresh) {
count_tr_ ++;
struct trackingRes new_tr_res_;
new_tr_res_.det_res.score = det_res[i].score;
new_tr_res_.det_res.cl = det_res[i].cl;
new_tr_res_.det_res.ct = det_res[i].ct;
new_tr_res_.det_res.tr = det_res[i].tr;
new_tr_res_.det_res.bb0 = det_res[i].bb0;
new_tr_res_.det_res.bb1 = det_res[i].bb1;
new_tr_res_.det_res.dep = det_res[i].dep;
new_tr_res_.det_res.dim[0] = det_res[i].dim[0];
new_tr_res_.det_res.dim[1] = det_res[i].dim[1];
new_tr_res_.det_res.dim[2] = det_res[i].dim[2];
new_tr_res_.det_res.alpha = det_res[i].alpha;
new_tr_res_.det_res.x = det_res[i].x;
new_tr_res_.det_res.y = det_res[i].y;
new_tr_res_.det_res.z = det_res[i].z;
new_tr_res_.det_res.rot_y = det_res[i].rot_y;
new_tr_res_.tracking_id = track_id[bi]++;
new_tr_res_.age = 1;
new_tr_res_.active = 1;
new_tr_res_.color = rand() % 256;
if(tr_res.size() <= bi) {
new_tr_res_.det_res.score = detRes[i].score;
new_tr_res_.det_res.cl = detRes[i].cl;
new_tr_res_.det_res.ct = detRes[i].ct;
new_tr_res_.det_res.tr = detRes[i].tr;
new_tr_res_.det_res.bb0 = detRes[i].bb0;
new_tr_res_.det_res.bb1 = detRes[i].bb1;
new_tr_res_.det_res.dep = detRes[i].dep;
new_tr_res_.det_res.dim[0] = detRes[i].dim[0];
new_tr_res_.det_res.dim[1] = detRes[i].dim[1];
new_tr_res_.det_res.dim[2] = detRes[i].dim[2];
new_tr_res_.det_res.alpha = detRes[i].alpha;
new_tr_res_.det_res.x = detRes[i].x;
new_tr_res_.det_res.y = detRes[i].y;
new_tr_res_.det_res.z = detRes[i].z;
new_tr_res_.det_res.rot_y = detRes[i].rot_y;
new_tr_res_.tracking_id = trackId[bi]++;
new_tr_res_.age = 1;
new_tr_res_.active = 1;
new_tr_res_.color = rand() % 256;
if(trRes.size() <= bi) {
std::vector<struct trackingRes> v_new_tr_res_;
v_new_tr_res_.push_back(new_tr_res_);
tr_res.push_back(v_new_tr_res_);
trRes.push_back(v_new_tr_res_);
}
else
tr_res[bi].push_back(new_tr_res_);
trRes[bi].push_back(new_tr_res_);
}
}
count_tr[bi] = count_tr_;
if(track_id[bi]==1000)
track_id[bi]=0;
det_res.clear();
countTr[bi] = count_tr_;
//reset the tracker id
if(trackId[bi] == 1000)
trackId[bi] = 0;
detRes.clear();
}
@@ -697,35 +686,36 @@ void CenternetDetection3DTrack::postprocess(const int bi, const bool mAP) {
// ---------------------------------- post-process -----------------------------------------
count_det = 0;
det_res.clear();
for(int i = 0; i<K; i++){
if(scores[i] < out_thresh)
countDet = 0;
detRes.clear();
for(int i=0; i<K; i++){
if(scores[i] < outThresh)
break;
count_det ++;
countDet ++;
struct detectionRes new_det_res;
new_det_res.score = scores[i];
new_det_res.cl = clses[i]+1;
// ret_s=scores[i];
// ret_c=clses[i]+1;
new_det_res.ct = transform_preds_with_trans(intxs[i], intys[i]);
new_det_res.tr = transform_preds_with_trans(intxs[i] + track[i], intys[i] + track[i+K]);
new_det_res.tr = new_det_res.tr -new_det_res.ct;
new_det_res.bb0 = transform_preds_with_trans(bbx0[i], bby0[i]);
new_det_res.bb1 = transform_preds_with_trans(bbx1[i], bby1[i]);
new_det_res.ct = transform_preds_with_trans(((bbx0[i]+bbx1[i])/2 + amodel_offset[i]),
new_det_res.ct = transform_preds_with_trans(intxs[i], intys[i]);
new_det_res.tr = transform_preds_with_trans(intxs[i] + track[i], intys[i] + track[i+K]);
new_det_res.tr = new_det_res.tr -new_det_res.ct;
new_det_res.bb0 = transform_preds_with_trans(bbx0[i], bby0[i]);
new_det_res.bb1 = transform_preds_with_trans(bbx1[i], bby1[i]);
new_det_res.ct = transform_preds_with_trans(((bbx0[i]+bbx1[i])/2 + amodel_offset[i]),
((bby0[i]+bby1[i])/2 + amodel_offset[i+K]));
new_det_res.dep = dep[i];
new_det_res.dep = dep[i];
new_det_res.dim[0] = dim_[i];
new_det_res.dim[1] = dim_[i+K];
new_det_res.dim[2] = dim_[i+2*K];
// unproject_2d_to_3d
new_det_res.z = dep[i] - calibs[bi].at<float>(2,3);
new_det_res.x = ((float)new_det_res.ct.at<float>(0,0) * dep[i] - calibs[bi].at<float>(0,3) - calibs[bi].at<float>(0,2) * new_det_res.z) / calibs[bi].at<float>(0,0);
new_det_res.y = ((float)new_det_res.ct.at<float>(0,1) * dep[i] - calibs[bi].at<float>(1,3) - calibs[bi].at<float>(1,2) * new_det_res.z) / calibs[bi].at<float>(1,1) + (dim_[i] / 2);
new_det_res.x = ((float)new_det_res.ct.at<float>(0,0) * dep[i] - calibs[bi].at<float>(0,3) -
calibs[bi].at<float>(0,2) * new_det_res.z) / calibs[bi].at<float>(0,0);
new_det_res.y = ((float)new_det_res.ct.at<float>(0,1) * dep[i] - calibs[bi].at<float>(1,3) -
calibs[bi].at<float>(1,2) * new_det_res.z) / calibs[bi].at<float>(1,1) + (dim_[i] / 2);
// alpha2rot_y
// idx = rot[:, 1] > rot[:, 5]
@@ -737,13 +727,11 @@ void CenternetDetection3DTrack::postprocess(const int bi, const bool mAP) {
else
new_det_res.alpha = std::atan2(rot[6*K + i], rot[7*K + i]) +0.5 * M_PI;
new_det_res.rot_y = (new_det_res.alpha + std::atan2((float)new_det_res.ct.at<float>(0,0) - calibs[bi].at<float>(0,2), calibs[bi].at<float>(0,0)));
new_det_res.ct = new_det_res.ct + new_det_res.tr; //dest
det_res.push_back(new_det_res);
new_det_res.ct = new_det_res.ct + new_det_res.tr; //dest
detRes.push_back(new_det_res);
}
// track step
tracking(bi);
batchTracked.push_back(tr_res[bi]);
}
void CenternetDetection3DTrack::draw(std::vector<cv::Mat>& frames) {
@@ -754,32 +742,38 @@ void CenternetDetection3DTrack::draw(std::vector<cv::Mat>& frames) {
int baseline = 0;
float font_scale = 0.8;
int thickness = 2;
for(int bi=0; bi<frames.size(); ++bi) {
float scale_x = float(originalSize[bi].width)/dim.w;
float scale_y = float(originalSize[bi].height)/dim.h;
resize(frames[bi], frames[bi], originalSize[bi]);
// draw dets
for(int i=0; tr_res.size() != 0 && i<tr_res[bi].size(); i++) {
t = tr_res[bi][i];
for(int i=0; trRes.size() != 0 && i<trRes[bi].size(); i++) {
t = trRes[bi][i];
id = t.tracking_id;
txt = classesNames[t.det_res.cl-1]+'-'+std::to_string(id); //forse ha bisogno di cl-1
cv::Size text_size = getTextSize(txt, cv::FONT_HERSHEY_SIMPLEX, font_scale, thickness, &baseline);
if(t.det_res.score > vis_thresh){// && t.active!=0) {
if(t.det_res.score > confThreshold){// && t.active!=0) {
if(view2d) {
cv::rectangle(frames[bi], cv::Point(t.det_res.bb0.at<float>(0,0), t.det_res.bb0.at<float>(0,1)),
cv::Point(t.det_res.bb1.at<float>(0,0), t.det_res.bb1.at<float>(0,1)), tr_colors[t.color], thickness);
cv::rectangle(frames[bi], cv::Point(t.det_res.bb0.at<float>(0,0),
t.det_res.bb0.at<float>(0,1) - text_size.height - thickness),
cv::Point(t.det_res.bb0.at<float>(0,0) + text_size.width,
t.det_res.bb0.at<float>(0,1)), tr_colors[t.color], -1);
cv::rectangle(frames[bi],
cv::Point(t.det_res.bb0.at<float>(0,0) * scale_x, t.det_res.bb0.at<float>(0,1) * scale_y),
cv::Point(t.det_res.bb1.at<float>(0,0) * scale_x, t.det_res.bb1.at<float>(0,1) * scale_y),
trColors[t.color], thickness);
cv::rectangle(frames[bi],
cv::Point(t.det_res.bb0.at<float>(0,0) * scale_x, t.det_res.bb0.at<float>(0,1) * scale_y - text_size.height - thickness),
cv::Point(t.det_res.bb0.at<float>(0,0) * scale_x + text_size.width, t.det_res.bb0.at<float>(0,1) * scale_y),
trColors[t.color], -1);
cv::putText(frames[bi], txt, cv::Point(t.det_res.bb0.at<float>(0,0),
t.det_res.bb0.at<float>(0,1) - thickness -1),
cv::FONT_HERSHEY_SIMPLEX, font_scale, cv::Scalar(255, 255, 255), 1);
cv::putText(frames[bi], txt,
cv::Point(t.det_res.bb0.at<float>(0,0) * scale_x, t.det_res.bb0.at<float>(0,1) * scale_y - thickness -1),
cv::FONT_HERSHEY_SIMPLEX, font_scale, cv::Scalar(255, 255, 255), 1);
cv::arrowedLine(frames[bi], cv::Point((int)t.det_res.ct.at<float>(0,0),
(int)t.det_res.ct.at<float>(0,1)),
cv::Point((int)(t.det_res.ct.at<float>(0,0) + t.det_res.tr.at<float>(0,0)),
(int)(t.det_res.ct.at<float>(0,1) + t.det_res.tr.at<float>(0,1))),
cv::Scalar(255, 0, 255), 2);
cv::arrowedLine(frames[bi],
cv::Point((int)t.det_res.ct.at<float>(0,0) * scale_x, (int)t.det_res.ct.at<float>(0,1) * scale_y),
cv::Point((int)(t.det_res.ct.at<float>(0,0) * scale_x + t.det_res.tr.at<float>(0,0) * scale_x),
(int)(t.det_res.ct.at<float>(0,1) * scale_y + t.det_res.tr.at<float>(0,1) * scale_y)),
cv::Scalar(255, 0, 255), 2);
}
//3d
if(!view2d && t.det_res.z > 1){
@@ -833,50 +827,59 @@ void CenternetDetection3DTrack::draw(std::vector<cv::Mat>& frames) {
res_corners.push_back(aus.at<float>(1,k) / aus.at<float>(2,k));
}
aus.release();
for(int ind_f = 3; ind_f>=0; ind_f--) {
for(int ind_f=3; ind_f>=0; ind_f--) {
for(int j=0; j<4; j++) {
cv::line(frames[bi], cv::Point((int)res_corners.at(face_id.at(ind_f).at(j) * 2),
(int)res_corners.at(face_id.at(ind_f).at(j) * 2 + 1)),
cv::Point((int)res_corners.at(face_id.at(ind_f).at((j+1)%4) * 2),
(int)res_corners.at(face_id.at(ind_f).at((j+1)%4) * 2 + 1)),
tr_colors[t.color], 2);
cv::line(frames[bi],
cv::Point((int)res_corners.at(faceId.at(ind_f).at(j) * 2) * scale_x,
(int)res_corners.at(faceId.at(ind_f).at(j) * 2 + 1) * scale_y),
cv::Point((int)res_corners.at(faceId.at(ind_f).at((j+1)%4) * 2) * scale_x,
(int)res_corners.at(faceId.at(ind_f).at((j+1)%4) * 2 + 1) * scale_y),
trColors[t.color], 2);
if(ind_f == 0 && j==3) {
cv::line(frames[bi], cv::Point((int)res_corners.at(face_id.at(ind_f).at(0) * 2),
(int)res_corners.at(face_id.at(ind_f).at(0) * 2 + 1)),
cv::Point((int)res_corners.at(face_id.at(ind_f).at(2) * 2),
(int)res_corners.at(face_id.at(ind_f).at(2) * 2 + 1)), tr_colors[t.color], 2);
cv::line(frames[bi], cv::Point((int)res_corners.at(face_id.at(ind_f).at(1) * 2),
(int)res_corners.at(face_id.at(ind_f).at(1) * 2 + 1)),
cv::Point((int)res_corners.at(face_id.at(ind_f).at(3) * 2),
(int)res_corners.at(face_id.at(ind_f).at(3) * 2 + 1)), tr_colors[t.color], 2);
cv::line(frames[bi],
cv::Point((int)res_corners.at(faceId.at(ind_f).at(0) * 2) * scale_x,
(int)res_corners.at(faceId.at(ind_f).at(0) * 2 + 1) * scale_y),
cv::Point((int)res_corners.at(faceId.at(ind_f).at(2) * 2) * scale_x,
(int)res_corners.at(faceId.at(ind_f).at(2) * 2 + 1) * scale_y), trColors[t.color], 2);
cv::line(frames[bi],
cv::Point((int)res_corners.at(faceId.at(ind_f).at(1) * 2) * scale_x,
(int)res_corners.at(faceId.at(ind_f).at(1) * 2 + 1) * scale_y),
cv::Point((int)res_corners.at(faceId.at(ind_f).at(3) * 2) * scale_x,
(int)res_corners.at(faceId.at(ind_f).at(3) * 2 + 1) * scale_y), trColors[t.color], 2);
}
}
}
float bb0=(1 << 10), bb1=0, bb2=(1 << 10), bb3=0;
for(int k=0; k<8; k++) {
if(res_corners[2*k]<bb0)
bb0=res_corners[2*k];
if(res_corners[2*k]>bb1)
bb1=res_corners[2*k];
if(res_corners[2*k+1]<bb2)
bb2=res_corners[2*k+1];
if(res_corners[2*k+1]>bb3)
bb3=res_corners[2*k+1];
if(res_corners[2*k] < bb0)
bb0 = res_corners[2*k];
if(res_corners[2*k] > bb1)
bb1 = res_corners[2*k];
if(res_corners[2*k+1] < bb2)
bb2 = res_corners[2*k+1];
if(res_corners[2*k+1] > bb3)
bb3 = res_corners[2*k+1];
}
// if(not no_bbox):
// cv::rectangle(frame, cv::Point(bb0, bb2), cv::Point(bb1, bb3),
// tr_colors[t.color], thickness);
cv::rectangle(frames[bi], cv::Point(bb0, bb2 - text_size.height - thickness),
cv::Point(bb0 + text_size.width, bb2), tr_colors[t.color], -1);
// cv::rectangle(frame,
// cv::Point(bb0, bb2),
// cv::Point(bb1, bb3),
// trColors[t.color], thickness);
cv::rectangle(frames[bi],
cv::Point(bb0 * scale_x, bb2 * scale_y - text_size.height - thickness),
cv::Point(bb0 * scale_x + text_size.width, bb2 * scale_y),
trColors[t.color], -1);
cv::putText(frames[bi], txt, cv::Point(bb0, bb2 - thickness -1), cv::FONT_HERSHEY_SIMPLEX,
font_scale, cv::Scalar(255, 255, 255), 1);
cv::putText(frames[bi], txt,
cv::Point(bb0 * scale_x, bb2 * scale_y - thickness -1),
cv::FONT_HERSHEY_SIMPLEX, font_scale, cv::Scalar(255, 255, 255), 1);
cv::arrowedLine(frames[bi], cv::Point((int)((bb0 + bb1)/2), (int)((bb2 + bb3)/2)),
cv::Point((int)((bb0 + bb1)/2 + t.det_res.tr.at<float>(0,0)),
(int)((bb2 + bb3)/2 + t.det_res.tr.at<float>(0,1))),
cv::Scalar(255, 0, 255), 2);
cv::arrowedLine(frames[bi],
cv::Point((int)((bb0 + bb1)/2) * scale_x, (int)((bb2 + bb3)/2) * scale_y),
cv::Point((int)((bb0 + bb1)/2 + t.det_res.tr.at<float>(0,0)) * scale_x,
(int)((bb2 + bb3)/2 + t.det_res.tr.at<float>(0,1)) * scale_y),
cv::Scalar(255, 0, 255), 2);
}
}
}