Hu Chunming / vpt_proj

#include <time.h>
#include "ErrorInfo.h"
#include "VPT.h"
#include "ObjCls.h"
#include "utools.h"

//model trt onnx 20210512
//#include "ga_vpt_onnx_net.h"
//#include "ga_trt_yolo_vpt_calibrator.h"

//model trt onnx 0715
//model pytorch_12cls_RenCheWu_yolov5m_hw640
// #include "ga_vpt_det_yolo_640x640.h"
// #include "ga_trt_yolo_vpt_calibrator.h"

//9
//老底层 模型
//#include "ga_vpt_predict_net.h"
//#include "ga_vpt_init_net.h"

//新trt的底层 跑caffe2的模型
//#include "ga_vpt_predict_net.h"
//#include "ga_vpt_init_net.h"

//13
//#include "ga_model.h"
//#include "ga_model_init.h"

#include "ga_vpt_yolov5m_onnx.h"
#include "ga_vpt_yolov5m_onnx_int8calibration.h"

//#define AUTHORIZATION

int checkTime()
{
	struct tm* info;
	int nYear, nMonth, nDay;
	time_t raw;
	time(&raw);
	info = localtime(&raw);
	nYear = info->tm_year + 1900;
	nMonth = info->tm_mon + 1;
	nDay = info->tm_mday;

	if (nYear == 2019 || (nYear == 2020 && nMonth < 4))
		return 1;
	else
	{
		printf("版本过期，请联系开发商！中科院自动化所模式识别实验室图像视频组\n");
		return -1;
	}
}

#include "authority.h"
//#define productSN "92DAF36635F7492EB40D4C10DC67832F"//人车物SDK_linux_v4.0.4.190701_cuda10.0_cudnn7.5.0
#define productSN "7CF8B4797F9E441686145BED07F662DE"//人车物SDK_linux_v4.0.4.190701_cuda10.0_cudnn7.5.0

map<int, int> index_mp= {make_pair(0,0), make_pair(1,2), make_pair(2,4), make_pair(4,2), make_pair(5,5), make_pair(6,6), make_pair(7,8), make_pair(8,3), make_pair(10,1) , make_pair(11,2), make_pair(12,7)};

void check_thread(objDetector * handle);

int VPT_Init(void **handle, VPT_PARAM vparam, char* resDir, VIDEO_OBJECT_SNAPSHOT_CALLBACK VideoObjSnapshotCallBack/* = NULL*/)
{
	//if (checkTime() != 1)
	//	return FAILED;

	int ret = SUCCESS;

#ifdef AUTHORIZATION
#ifdef _WIN32
  if(SUCCESS == (ret = sy_licence(productSN)))
#elif __linux__
			char wtime[15];
	memset(wtime, 0, 15);
	char * time = wtime;
	if (SUCCESS == (ret = sy_licence(productSN, &time)))
#endif
#endif
  {
  	*handle = new objDetector;	//申请对象
  	memcpy((void *)&((objDetector *)*handle)->param, (void *)&vparam, sizeof(VPT_PARAM));
  
  	int flag = 0;	//返回值 默认为SUCCEEDED：0
  
    // pytorch_12cls_RenCheWu_yolov5m_hw640,模型类别有变化，process直接映射回之前的index
    ctools_init_params vpt_param;
	// vpt_param.thres_ = 0.5;//vparam.thres;
	vpt_param.thres_ = vparam.thres; // 对外开放
	vpt_param.log_level_ = 0;
	vpt_param.device_type_ =DEVICE_GPU;
	vpt_param.device_id_ =  vparam.gpuid;
	vpt_param.engine_type_ = ENGINE_TENSORRT;
	vpt_param.data_process_str_ = 
   		"CopyData_CPU2GPU_U8;"
		"TypeConvert_U8_F32;"
		"ResizeMaxMidPad_F32_F32,test_size,640,test_max_size,640,max_height,640,max_width,640,"
		"submean_b,0,submean_g,0,submean_r,0,"
		"variance_rev_b,0.00392,variance_rev_g,0.00392,variance_rev_r,0.00392;"
		"BGR2RGB_F32_F32;"
		"NHWC2NCHW_F32"
		;
    //param.preprocess_param;
	if (vpt_param.engine_type_ == ENGINE_MCAFFE2)
	{
		printf("ENGINE_MCAFFE2 not support!\n");
      	vpt_param.engine_type_ = ENGINE_TENSORRT;
	}
	 
   	if (vpt_param.engine_type_ == ENGINE_TENSORRT)
	{
		vpt_param.need_im_info_ = 0; // true
		vpt_param.model_type_ = MODEL_YOLOV5;
			
		vpt_param.net_array_ = (uint8_t*)ga_vpt_yolov5m_onnx;
		vpt_param.net_array_len_ = ga_vpt_yolov5m_onnx_len;

      	char trt_serialize_file_[1024];
      	{
        	if(vparam.serialize_file == nullptr) vparam.serialize_file = ".";
       		string dir_path =  vparam.serialize_file;
        	int iLen = dir_path.length();
       
        	if (dir_path[iLen - 1] != '\\' && dir_path[iLen - 1] != '/')
			{
				dir_path += '/';
			}
	        sprintf(trt_serialize_file_, "%sVPT_DET_MODEL20220211", dir_path.c_str());
     	}
      	printf("serialize_file path: %s\n",trt_serialize_file_ );
		vpt_param.trt_serialize_file_ = trt_serialize_file_;//"VPT_DET";
		memset(vpt_param.tensorrt_param_str_, 0, sizeof(vpt_param.tensorrt_param_str_));
		int batch_size = vparam.max_batch; //
		std::string g_data_mode = "INT8";
		bool g_is_create_calibrator = false;
		int g_is_onnx_model = 1;

		sprintf(vpt_param.tensorrt_param_str_, "max_batchsize %d,"
			"data_mode %s,"
			"is_create_calibrator %d,"
			"is_onnx_model %d,"
			"input_names images,"
			"output_names output",
		batch_size, g_data_mode.c_str(), g_is_create_calibrator, g_is_onnx_model);

		//vpt_param.tensorrt_calibrator_file_ = "trt_yolo_vpt_calibrator";
		vpt_param.tensorrt_calibrator_array_len_ = ga_vpt_yolov5m_onnx_int8calibration_len;// "trt_fpn_vpt_calibrator";
		vpt_param.tensorrt_calibrator_array_ = (unsigned char*)ga_vpt_yolov5m_onnx_int8calibration;// "trt_fpn_vpt_calibrator";
	}
	ret = ctools_init(&(((objDetector *)*handle)->detector), &vpt_param);

    if(ret == SUCCESS)
    {
    	//----------------- 9类结构化（自用单路人车物） ---------------------//
    	//拌线初始化
    	// ((objDetector *)*handle)->tracker = new Sort(/*lineArray, linecount, DETECTTYPE*/);
    	((objDetector *)*handle)->snapshotHelper = new SnapShot(resDir, VideoObjSnapshotCallBack, vparam.SnaoshotParameter);
		((objDetector *)*handle)->frameCounter_ = 0; // 记录帧号 220826 byzsh
    
    	((objDetector *)*handle)->frameCounter = FusionInterval;
     	((objDetector *)*handle)->isInitFrame = true;
      
		//----------------- 授权线程初始化 ---------------------//
#ifdef AUTHORIZATION
		((objDetector *)*handle)->licence_status = 0;
			((objDetector *)*handle)->thrd = boost::thread(check_thread, ((objDetector *)*handle));
		((objDetector *)*handle)->thrd_status = 0;
#endif
    }
  }
/*
  }
  else
  {
    ret =  AUTHOR_ERROR;	
  }
#ifdef __linux__
  if(wtime)
  {
		delete[] wtime;
		wtime = NULL;
  }
#endif
*/
  return ret;
}
  

/* 任务添加跟踪器 */
void AddTaskTracker(void * handle, const char* taskID, double rWidth=1, double rHeight=1)
{
	objDetector *tools = (objDetector*)handle;
	TaskTracker t;
	t.TaskID = taskID;
	t.ratioWidth = rWidth;
	t.ratioHeight = rHeight;
	tools->taskTrackers[taskID] = t;
}

/* 任务结束跟踪器 */
bool FinishTaskTracker(void * handle, const char* taskID)
{
	objDetector *tools = (objDetector*)handle;
	tools->taskTrackers.erase(taskID);
	return true;
}

/* 任务暂停跟踪器 */
void PauseTaskTracker(void * handle, const char* taskID)
{
	objDetector *tools = (objDetector*)handle;
	tools->taskTrackers[taskID].tracker.Pause();
}

/* 任务重新开启跟踪器 */
void RestartTaskTracker(void * handle, const char* taskID)
{
	objDetector *tools = (objDetector*)handle;
	tools->taskTrackers[taskID].tracker.ReSet();

}


//todo----------------
void VPT_ResetTracker(void * handle/*, int maxResultCountVPT_Line * lineArray = NULL, int linecount = 0*/)
{	
	objDetector* tools = (objDetector*)handle;
	// tools->tracker->Release();
	// delete tools->tracker;
	// tools->tracker = NULL; 
	// tools->tracker = new Sort(/*lineArray, linecount, DETECTTYPE*/);	//新接口 无需流量统计和快照
	for(auto ss = tools->taskTrackers.begin(); ss != tools->taskTrackers.end(); ) {
		Sort &cur_sort = tools->taskTrackers[ss->first].tracker;
		cur_sort.Release();
		tools->taskTrackers.erase(ss++);
	}
}


//辅助函数：数据预处理
float VPTmeanSub[3] = { 103.52, 116.28, 123.675 };
float VPTVariance[3] = { 57.375, 57.12, 58.395 };
void datapreprocess(float * blob, int width, int height, int channels, int batchsize)
{

	int datasize_ = width * height;
	for (int c = 0; c < 3; c++)
	{
		for (int i = 0; i < width * height; i++)
		{
			blob[datasize_ * c + i] = ((blob[datasize_ * c + i]) - VPTmeanSub[c]) / VPTVariance[c];
		}
	}

}


int VPT_ProcessLastFrame(void * handle/*, unsigned char * rgb, int width, int height, FT_Result * result*/)
{
	objDetector* tools = (objDetector*)handle;

	/*vector< vector <float>> detectResult;	//转化为跟踪所需要的输入
	int index = 0;
	bool isUseDet = false;
	vector<int> deleteTrackers;

	int ObjCount = tools->tracker->updateLastFrame(deleteTrackers);

	//void SaveSnapshot(vector<int> deleteTrackers);
	tools->snapshotHelper->SaveSnapshot(deleteTrackers);
	sleep(10); //等待一会，以便存完 220824 byzsh
	vector<vector<float>>().swap(detectResult);
	detectResult.clear();

	vector<int>().swap(deleteTrackers);
	deleteTrackers.clear();*/

	// 220826 byzsh
	for(auto iter_tracker : tools->taskTrackers)
	{
		if (!iter_tracker.second.tracker.GetState()) ////
			continue;

		Sort &cur_sort = tools->taskTrackers[iter_tracker.first].tracker;

		vector<int> deleteTrackers;
		int ObjCount = cur_sort.updateLastFrame(deleteTrackers); //
		// tools->snapshotHelper->SaveSnapshot(deleteTrackers);
		tools->snapshotHelper->SaveSnapshotV2(deleteTrackers, iter_tracker.first);
		
		vector<int>().swap(deleteTrackers);
		deleteTrackers.clear();
	}
	sleep(10); //等待一会，以便存完 220824 byzsh
	return 0;
}

//int VPT_Process(void * handle, unsigned char *bgr, int width, int height, int channels, int frameCount, VPT_Result *result, vector<int> &deleteTrackers, VPT_TrafficResult* traffic, bool showTrack)
int VPT_Process(void * handle, unsigned char *bgr, int width, int height, int channels, int frameCount, VPT_Result *result,/* int maxResultCount,*/ bool showTrack)
{
	objDetector* tools = (objDetector*)handle;
	//CDTOOLS_Result ssdResult[MAX_OBJ_COUNT_TEST];
  
  #ifdef AUTHORIZATION
  if (tools->licence_status > -3)
  #endif
  {
  	// 有新任务则添加跟踪器
	const char* video_key_ = "single_task";
	if(tools->taskTrackers.find(video_key_) == tools->taskTrackers.end()) {
		AddTaskTracker(tools, video_key_);
	}

	Mat img(height, width, CV_8UC3, bgr);
    cv::Mat tmp_img;
    img.copyTo(tmp_img);
  	vector< vector <float>> detectResult;	//转化为跟踪所需要的输入
  	int index = 0;
  	bool isUseDet = false;
  	vector<int> deleteTrackers;
  
  	if (tools->frameCounter == FusionInterval || tools->isInitFrame)
  	{
  	//	cout << "begin Ctools_Detector" << endl;
  
  		sy_img cur_img;
  		cur_img.set_data(img.cols, img.rows, img.channels(), img.data);
  
  		ctools_result *detresult;
  
  		int res_status = ctools_process(tools->detector, &cur_img, 1, &detresult);
  
  		//cout << "finish Ctools_Detector" << endl;
  
  
  		ctools_result &cur_result = detresult[0];
  
  		//(*result)[b].obj_count_ = cur_result.obj_count_ > MAX_DET_COUNT ? MAX_DET_COUNT : cur_result.obj_count_;
  
  		for (int c = 0; c < cur_result.obj_count_ && c < tools->param.maxResultCount; c++)
  		{
  			float score = cur_result.obj_results_[c].data_[1];
        	int old_index = cur_result.obj_results_[c].data_[0];
      
  			if (score >= THRESHOLD && (old_index!=3 && old_index!=9)/*index 为pytorch_12cls_RenCheWu_yolov5m_hw640模型添加index映射的操作*/)  //更改为left top right bottom score index(未-1的原始数据 sort内部进行-1操作)
  			{
  				vector <float> obj;
  				obj.push_back(cur_result.obj_results_[c].data_[2]);
  				obj.push_back(cur_result.obj_results_[c].data_[3]);
  				obj.push_back(cur_result.obj_results_[c].data_[4]);
  				obj.push_back(cur_result.obj_results_[c].data_[5]);
  				obj.push_back(cur_result.obj_results_[c].data_[1]);
         		//obj.push_back(cur_result.obj_results_[c].data_[0] + 1);  //针对YOLO模型 内部-1 外部得+1
          
  				int new_index = index_mp[old_index] + 1;
          		obj.push_back(new_index);
        
  				detectResult.push_back(obj);
  			}
  		}
  
  		//dResult->objCount = index;
  		result->objCount = detectResult.size();
     
  		//VPT_Result temp;
  		tools->isInitFrame = false;
  		tools->frameCounter = 0;
  		isUseDet = true;
  
  	}
  	tools->frameCounter++;
  
  
  	//for (int j = 0; j < FusionInterval; j++)	//跳帧：检测一帧，跟踪 (FusionInterval - 1)帧 
  	//{
   
  	int objCount = tools->taskTrackers[video_key_].tracker.update(width, height, isUseDet, detectResult, result->obj, deleteTrackers);
  
  	result->objCount = objCount;
	if (showTrack)
  		tools->taskTrackers[video_key_].tracker.addTracker(&tmp_img);
  	
  	// tools->snapshotHelper->SnapshotProcess(result, tmp_img, deleteTrackers, frameCount);
	tools->snapshotHelper->SnapshotProcessV2(result, tmp_img, deleteTrackers, frameCount, video_key_); 
  
    vector<vector<float>>().swap(detectResult);
  	detectResult.clear();
  	
  	vector<int>().swap(deleteTrackers);
  	deleteTrackers.clear();
  	//}
  //}
  
  
  	// if (showTrack)
  	// 	tools->taskTrackers[video_key_].tracker.addTracker(&tmp_img);
 
  	return 0;
  }
  /*
	else
	{
    return AUTHOR_ERROR;
	}
 */
}


// 220824 byzsh
int VPT_Process_batch(void * handle, xjs_batch_img *bgrs, int batchsize, VPT_Result **result, bool showTrack)
{
	objDetector* tools = (objDetector*)handle;
	
#ifdef AUTHORIZATION
  if (tools->licence_status > -3)
#endif
  {
  	vector <vector< vector <float>>> detectResult(batchsize);	//转化为跟踪所需要的输入
  	int index = 0;
  	bool isUseDet = false;
  	vector <vector<int>> deleteTrackers(batchsize);
  
  	if (tools->frameCounter == FusionInterval || tools->isInitFrame) // 内部跳帧
  	{
  		// cout << "begin Ctools_Detector" << endl;
		sy_img *batch_img = new sy_img[batchsize];
    	for (int idx = 0; idx < batchsize; idx++) 
    	{
      		// 有新任务则添加跟踪器
			if(tools->taskTrackers.find(bgrs[idx].video_key_) == tools->taskTrackers.end()) {
				AddTaskTracker(tools, bgrs[idx].video_key_);
			}
			  	
			batch_img[idx].set_data(bgrs[idx].w_, bgrs[idx].h_, bgrs[idx].c_, (unsigned char *)bgrs[idx].data_);
    	}
		
		ctools_result *detresult;
		int res_status = ctools_process(tools->detector, batch_img, batchsize, &detresult);
  		// cout << "finish Ctools_Detector" << endl;
  
		for (int b = 0; b < batchsize; b++)
		{
			ctools_result &cur_result = detresult[b];
			for (int c = 0; c < cur_result.obj_count_ && c < tools->param.maxResultCount; c++)
			{
				float score = cur_result.obj_results_[c].data_[1];
        		int old_index = cur_result.obj_results_[c].data_[0];
				if (score >= THRESHOLD && (old_index!=3 && old_index!=9))  //更改为left top right bottom score index(未-1的原始数据 sort内部进行-1操作)
				{
					vector <float> obj;
					obj.push_back(cur_result.obj_results_[c].data_[2]);
					obj.push_back(cur_result.obj_results_[c].data_[3]);
					obj.push_back(cur_result.obj_results_[c].data_[4]);
					obj.push_back(cur_result.obj_results_[c].data_[5]);
					obj.push_back(cur_result.obj_results_[c].data_[1]);
					//obj.push_back(cur_result.obj_results_[c].data_[0] + 1);  //针对YOLO模型 内部-1 外部得+1
			
					int new_index = index_mp[old_index] + 1;
					obj.push_back(new_index);
			
					detectResult[b].push_back(obj);
				}
			}
			// (*result)[b].objCount = detectResult[b].size();
		}
	
  		//VPT_Result temp;
  		tools->isInitFrame = false;
  		tools->frameCounter = 0;
  		isUseDet = true;

		if (batch_img != nullptr) {
			delete[] batch_img;
			batch_img = nullptr;
		}
  
  	}
  	tools->frameCounter++; 
  
	/* 跟踪 */
	// Update Tracker Result.
	/*
	int detectIndex = 0;
	for(auto iter_tracker : tools->taskTrackers)
	{
		// cout << "detectIndex:" << detectIndex << " " << iter_tracker.first << endl;
		if (!iter_tracker.second.tracker.GetState())
			continue;

		if(bgrs[detectIndex].video_key_ != iter_tracker.first)
			printf("index error!\n");

		Mat img(bgrs[detectIndex].h_, bgrs[detectIndex].w_, CV_8UC3, bgrs[detectIndex].data_);
		cv::Mat tmp_img;
		img.copyTo(tmp_img);
		Sort &cur_sort = tools->taskTrackers[iter_tracker.first].tracker;
		int objCount = cur_sort.update(bgrs[detectIndex].w_, bgrs[detectIndex].h_, isUseDet, detectResult[detectIndex], (*result)[detectIndex].obj, deleteTrackers[detectIndex]);
		(*result)[detectIndex].objCount = objCount;
		tools->snapshotHelper->SnapshotProcessV2(&(*result)[detectIndex], tmp_img, deleteTrackers[detectIndex], bgrs[detectIndex].frameCount_, bgrs[detectIndex].video_key_); // 抓拍 todo
		vector<vector<float>>().swap(detectResult[detectIndex]);
		detectResult[detectIndex].clear();
		vector<int>().swap(deleteTrackers[detectIndex]);
  		deleteTrackers[detectIndex].clear();
		if (showTrack)
  			cur_sort.addTracker(&tmp_img);

		++detectIndex;
	}*/

	for(int detectIndex = 0; detectIndex < batchsize; detectIndex++)
	{
		// cout << "detectIndex:" << detectIndex << " " << bgrs[detectIndex].video_key_ << endl;
		if(tools->taskTrackers.find(bgrs[detectIndex].video_key_) == tools->taskTrackers.end()) {
			printf("[ERROR] video_key_:%s was not added to tackers!", bgrs[detectIndex].video_key_);
			return -1;
		}
		
		Sort &cur_sort = tools->taskTrackers[bgrs[detectIndex].video_key_].tracker;

		if (!cur_sort.GetState())
			continue;

		Mat img(bgrs[detectIndex].h_, bgrs[detectIndex].w_, CV_8UC3, bgrs[detectIndex].data_);
		cv::Mat tmp_img;
		img.copyTo(tmp_img);
		
		int objCount = cur_sort.update(bgrs[detectIndex].w_, bgrs[detectIndex].h_, isUseDet, detectResult[detectIndex], (*result)[detectIndex].obj, deleteTrackers[detectIndex]);
		(*result)[detectIndex].objCount = objCount;
		if (showTrack)
  			cur_sort.addTracker(&tmp_img);
		tools->snapshotHelper->SnapshotProcessV2(&(*result)[detectIndex], tmp_img, deleteTrackers[detectIndex], bgrs[detectIndex].frameCount_, bgrs[detectIndex].video_key_); // 抓拍 todo
		vector<vector<float>>().swap(detectResult[detectIndex]);
		detectResult[detectIndex].clear();
		vector<int>().swap(deleteTrackers[detectIndex]);
  		deleteTrackers[detectIndex].clear();
		// if (showTrack)
  		// 	cur_sort.addTracker(&tmp_img);

		cur_sort.frame_count_ = tools->frameCounter_;

	}

	vector<vector<vector<float>>>().swap(detectResult);
	vector<vector<int>>().swap(deleteTrackers);

	
	// 销毁一直未出现任务的跟踪器
	for(auto ss = tools->taskTrackers.begin(); ss != tools->taskTrackers.end(); ) {
		bool tracker_flag = true;
		for (int b = 0; b < batchsize; b++) {
			if (bgrs[b].video_key_ == ss->first)
				tracker_flag = false;
		}
		if (tracker_flag) { // 若跟踪器里的任务没出现，则根据帧号判断时间间隔
		    Sort &cur_sort = tools->taskTrackers[ss->first].tracker;
			if (abs(tools->frameCounter_ - cur_sort.frame_count_)  > WAITFRAMES) { // 超出最大间隔则销毁
				// cout << "task is finish:" << ss->first << " " << tools->frameCounter_ - cur_sort.frame_count_ << endl;
				vector<int> deleteTrackers;
				int ObjCount = cur_sort.updateLastFrame(deleteTrackers); //
				tools->snapshotHelper->SaveSnapshotV2(deleteTrackers, ss->first);
				
				vector<int>().swap(deleteTrackers);
				deleteTrackers.clear();
				tools->taskTrackers.erase(ss++);
			}
			else ss++;
		}
		else {
			ss++;
		}
		
	}

	tools->frameCounter_++;
	
  	return 0;
  }
 
}




void VPT_Release(void **handle)
{
	objDetector* tools = (objDetector*)*handle;

	VPT_ProcessLastFrame(tools);

	if (tools && tools->detector)
	{
		ctools_release(&tools->detector);
		tools->detector = NULL;
	}

	for(auto ss = tools->taskTrackers.begin(); ss != tools->taskTrackers.end(); ) {
		// cout << ss->first << endl;
		Sort &cur_sort = tools->taskTrackers[ss->first].tracker;
		cur_sort.Release();
		tools->taskTrackers.erase(ss++);
	}
 
 #ifdef AUTHORIZATION
  if(tools->thrd_status == 0)
  {
			tools->thrd.interrupt();
			tools->thrd.join();
			tools->thrd_status = -1;
	}
 #endif
	if (tools)
	{
		delete tools;
		tools = NULL;
	}
}

	void check_thread(objDetector * handle)
		{
			int res = -1;
#ifdef __linux__
			char wtime[15];
	memset(wtime, 0, 15);
	char * time = wtime;
#endif
			while (1)
			{
				//printf("xxx check status on process...\n");
#ifdef _WIN32
				res = sy_licence(productSN);
#elif __linux__
				res = sy_licence(productSN, &time);
				//printf("--------------wtime in thread: %s, status: %d\n", wtime, licence_status);
#endif
				if (res < 0)
				{
					handle->licence_status= handle->licence_status - 1;
				}
				else
				{
					if (handle->licence_status < 0)
					{
						handle->licence_status = 0;
					}
				}
				boost::this_thread::sleep(boost::posix_time::seconds(300)); //5min
				//boost::this_thread::sleep(boost::posix_time::milliseconds(10);
			}
		}