331 lines
8.6 KiB
C++
Executable File
331 lines
8.6 KiB
C++
Executable File
#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;
|
||
} |