image_framework_ymj/image_framework/plugins/PluginCameraProcessing.cpp

#include "PluginCameraProcessing.h"
#include "MsgBase.h"
#include "Libapi3d.h"
#include "Libapi2d.h"
#include "Libapi.h"
#include "Log.h"
#include "IniHelper.h"


static int CapHqImage(CMeasureInfo* measureInfoPtr)
{
	int ret = CAL_OK;

	std::shared_ptr<cv::Mat> hqImPtr = std::make_shared<cv::Mat>();
	hqImPtr.get()->create(7000, 9344, CV_8UC3);

	ret = Cal2D::LibapiCapHQImage(measureInfoPtr, hqImPtr);

	return ret;
}

static int DetectObjs(CMeasureInfo* measureInfoPtr)
{
	int ret = CAL_OK;

	// 通过 yolo 检测 预埋件的位置，输出rois（n * roiPoints * 4）
	ret = Cal2D::LibapiDetectObj(measureInfoPtr);

	return ret;
}

static int CalConners2D(CMeasureInfo* measureInfoPtr)
{
	int ret = CAL_OK;

	// 分发线程ROI检测任务（内角点粗略定位->内角点精确定位，每个任务： 1 * innerConners2D * 4s）
	int roi_size = measureInfoPtr->GetSubRois().size();
	if (roi_size == 0)
	{
		Loger(LEVEL_INFOR, "roi_size = measureInfoPtr->GetSubRois().size() == 0");
		return CAL_YOLO_DETECT_NO_ROI;
		//assert(0);
	}

	int roi_index = 0;

	// 初始化所有SubRoi
	for (int i = 0; i < roi_size; ++i)
	{
		std::shared_ptr<CSubRoi> subRoiPtr = measureInfoPtr->GetSubRois()[i];
		subRoiPtr->mPparentId = measureInfoPtr->GetID();
		long id = -1;
		CalUtils::LibapiGenRandomId(id);
		subRoiPtr->mId = id;
		subRoiPtr->mInfo = measureInfoPtr->GetInfo();
		subRoiPtr->isGood = true;
	}

	while (true)
	{
		MsgBase* pMsgRoi = nullptr;
		char msg_type[32] = "v0.1";
		libapi_create_MsgBase(msg_type, (void**)&pMsgRoi);
		pMsgRoi->mRoiCounts = roi_size;
		pMsgRoi->mRoiIndex = roi_index;
		pMsgRoi->mMsgType = "roi_start";

		ret = libapi_push_to_queue((char*)"_t_PluginRoiStart", pMsgRoi);
		if (ret != CAL_OK)
		{
			Loger(LEVEL_INFOR, "libapi_push_to_queue((char*)\"_t_PluginRoiStart\", pMsgRoi) error");
			return CAL_ROIS_PUSH_TREAD_FAILED;
			//assert(0);
		}

		// 待优化
		++roi_index;
		if (roi_size == roi_index || roi_size >= 16)
			break;
	}

	// 等待所有ROI任务结束
	MsgBase::WaitBool(MsgBase::bAllRoisConnersCheckIsEnd);
	Loger(LEVEL_INFOR, "MsgBase::WaitBool(MsgBase::bAllRoisConnersCheckIsEnd)");
	return ret;
}

static int CalConners3D(CMeasureInfo* measureInfoPtr)
{
	int ret = CAL_OK;

	int roi_size = measureInfoPtr->GetSubRois().size();

	// 分发线程计算H矩阵并计算各个subMat的 innerConners3D 数据
	for (int i = 0; i < roi_size; ++i)
	{
		CSubRoi* subRoi = measureInfoPtr->GetSubRois()[i].get();
		cv::Mat H;

		if (!subRoi->isGood)
			continue;

		ret = Cal2D::LibapiCalH(subRoi->mVc2D, subRoi->mVc3D, H);
		if (ret != CAL_OK)
		{
			subRoi->isGood = false;
			ret = CAL_OK;
			continue;
			//return CAL_H_FAILED;
			//assert(0);
		}

		ret = Cal3D::LibapiCalConner3D(subRoi->mInnerConners, H, subRoi->mVc3D, subRoi->mInnerConners3D);
		if (ret != CAL_OK)
		{
			return CAL_3D_COONERS_FAILED;
			//assert(0);
		}
	}
	return ret;
}

static int CalData(CMeasureInfo* measureInfoPtr)
{
	int ret = CAL_OK;

	return ret;
}

static int InitLog()
{
	int ret = CAL_OK;
	// 初始化log
	std::string infoLogPath = "ERROR";
	Log::getInstance().init(LEVEL_INFOR, infoLogPath.c_str());
	Loger(LEVEL_INFOR, IniHelper::Get()->GetRecord().c_str());
	return 0;
}

static bool hasGoodConners(CMeasureInfo* measureInfoPtr)
{
	for (int i = 0; i < measureInfoPtr->GetSubRois().size(); ++i)
		if (measureInfoPtr->GetSubRois()[i].get()->isGood)
			return true;
	return false;
}

static int pythonProcessing(CMeasureInfo* measureInfoPtr)
{
	int ret = CAL_OK;

	DoCallback(TO_CLIENT_NOTIFY_DETECTION_START);
	ret = DetectObjs(measureInfoPtr);
	if (ret != CAL_OK)
	{
		Loger(LEVEL_INFOR, "[ERROR] DetectObjs is error!");
		DoCallback(ret);
		CMeasureInfo::Clear();
		return ret;
		//assert(0);
	}
	DoCallbackWithData(TO_CLIENT_NOTIFY_DETECTION_END);
	Loger(LEVEL_INFOR, "DetectObjs is success!");

	return ret;
}

PluginCameraProcessing::PluginCameraProcessing() {


};

PluginCameraProcessing::~PluginCameraProcessing()
{

};

int PluginCameraProcessing::OnProcess(LibapiThread* pThisThread, void* pMsg)
{
	int ret = CAL_OK;

	MsgBase* pCameraMsg = (MsgBase*)pMsg;
	CMeasureInfo* measureInfoPtr = CMeasureInfo::Init(1, "test");
	bool bCircle = true;

	// 处理广角图片
	if (pCameraMsg->mMsgType == "PythonProcessing")
	{
		Param p = pCameraMsg->params[PARAM_POS_SRC_IMAGE];
		std::shared_ptr<cv::Mat> im = std::make_shared<cv::Mat>();
		im->create(p.height, p.width, CV_8UC3);
		memcpy(im.get()->data, (char*)p.spData.get(), p.height * p.width * 3);
		Loger(LEVEL_INFOR, " cap image size is w: %d, h: %d", p.height, p.width);
		measureInfoPtr->SetIm(im);
		Loger(LEVEL_INFOR, "get image");
		return pythonProcessing(measureInfoPtr);
	}

	InitLog();

	while (bCircle)
	{
		std::chrono::high_resolution_clock::time_point m_start = std::chrono::high_resolution_clock::now();
		DoCallback(TO_CLIENT_NOTIFY_CAMERA_CAP_START);
		ret = CapHqImage(measureInfoPtr);
		if (ret != CAL_OK)
		{
			Loger(LEVEL_INFOR, "[ERROR] CapHqImage is error!");
			MsgBase::SetWaitInt(MsgBase::iCameraToLidarMsg, CLIENT_MSG_STOP_DETECTION);
			DoCallback(ret);
			CMeasureInfo::Clear();
			return ret;
			//assert(0);
		}
		DoCallback(TO_CLIENT_NOTIFY_CAMERA_CAP_END);
		Loger(LEVEL_INFOR, "CapHqImage is success!");


		DoCallback(TO_CLIENT_NOTIFY_DETECTION_START);
		ret = DetectObjs(measureInfoPtr);
		if (ret != CAL_OK)
		{
			Loger(LEVEL_INFOR, "[ERROR] DetectObjs is error!");
			MsgBase::SetWaitInt(MsgBase::iCameraToLidarMsg, CLIENT_MSG_STOP_DETECTION);
			DoCallback(ret);
			CMeasureInfo::Clear();
			return ret;
			//assert(0);
		}
		DoCallbackWithData(TO_CLIENT_NOTIFY_DETECTION_END);
		Loger(LEVEL_INFOR, "DetectObjs is success!");


		DoCallback(TO_CLIENT_NOTIFY_CONNER2D_START);
		ret = CalConners2D(measureInfoPtr);
		if (ret != CAL_OK)
		{
			Loger(LEVEL_INFOR, "[ERROR] CalConners2D is error!");
			MsgBase::SetWaitInt(MsgBase::iCameraToLidarMsg, CLIENT_MSG_STOP_DETECTION);
			DoCallback(ret);
			CMeasureInfo::Clear();
			return ret;
			//assert(0);
		}
		if (!hasGoodConners(measureInfoPtr))
		{
			MsgBase::SetWaitInt(MsgBase::iCameraToLidarMsg, CLIENT_MSG_STOP_DETECTION);
			DoCallback(CAL_HAS_NO_GOOD_CONNER);
			CMeasureInfo::Clear();
			Loger(LEVEL_INFOR, "[ERROR] CalConners2D has no good conner !");
			return ret;
			//assert(0);
		}
		Loger(LEVEL_INFOR, "CalConners2D is success!");
		DoCallbackWithData(TO_CLIENT_NOTIFY_CONNER2D_END);

		// send msg to client
		// wait client nake sure
		int msg_rst = MsgBase::WaitInt(MsgBase::iClinetMsg);
		switch (msg_rst)
		{
		case CLIENT_MSG_NONE:
			assert(0);
		case CLIENT_MSG_CONTINUE:
			MsgBase::SetWaitInt(MsgBase::iCameraToLidarMsg, CLIENT_MSG_CONTINUE);
			bCircle = false;
			break;
		case CLIENT_MSG_RESTART_DETECTION:
			MsgBase::SetWaitInt(MsgBase::iCameraToLidarMsg, CLIENT_MSG_RESTART_DETECTION);
			CMeasureInfo::Clear();
			continue;
		case CLIENT_MSG_STOP_DETECTION:
			MsgBase::SetWaitInt(MsgBase::iCameraToLidarMsg, CLIENT_MSG_STOP_DETECTION);
			CMeasureInfo::Clear();
			return ret;
		default:
			assert(0);
		}
		DoCallback(TO_CLIENT_NOTIFY_CONNER3D_START);

		// 等待点云处理完成
		MsgBase::WaitBool(MsgBase::bCloudProjectIsOk);
		if (measureInfoPtr->GetIm2D3D() == nullptr)
		{
			DoCallback(CAL_PRAMA_EMPUTY);
			CMeasureInfo::Clear();
			return CAL_PRAMA_EMPUTY;
		}

		ret = CalConners3D(measureInfoPtr);
		if (ret != CAL_OK)
		{
			DoCallback(ret);
			CMeasureInfo::Clear();
			Loger(LEVEL_INFOR, "[ERROR] CalConners3D is error!");
			return ret;
			//assert(0);
		}
		DoCallback(TO_CLIENT_NOTIFY_CONNER3D_END);
		Loger(LEVEL_INFOR, "CalConners3D is success!");

		DoCallback(TO_CLIENT_NOTIFY_MEASURE_START);
		ret = CalData(measureInfoPtr);
		if (ret != CAL_OK)
		{
			DoCallback(ret);
			CMeasureInfo::Clear();
			Loger(LEVEL_INFOR, "[ERROR] CalData is error!");
			return ret;
			//assert(0);
		}
		Loger(LEVEL_INFOR, "CalData is success!");
		DoCallbackWithData(TO_CLIENT_NOTIFY_MEASURE_END);

		// 清楚当此所有信息
		CMeasureInfo::Clear();

		std::chrono::high_resolution_clock::time_point m_end = std::chrono::high_resolution_clock::now();
		std::chrono::duration<double> elapsex = m_end - m_start;
		std::cout << "PluginCameraProcessing::OnProcess time is : " << elapsex.count() << std::endl;
	};

	Log::getInstance().quit();
	return ret;
}

void PluginCameraProcessing::OnEndProcess(LibapiThread* pThisThread, void* pMsg)
{
	MACRO_RECORD_RUN_TIME(pThisThread->get_name());
	//std::cout << " PluginCameraProcessing is end! " << std::endl;
}