460 lines
15 KiB
C++
460 lines
15 KiB
C++
#include "Hd_3DCameraLMI_module.h"
|
|
#include <QDebug>
|
|
QQueue<std::vector<cv::Mat>> queuePic;
|
|
QMutex* m_mutex;
|
|
bool triggerIng = false;
|
|
bool recivingImage = false,frameStart = false;
|
|
int lastFrameId = -1 ;
|
|
long long frameStampCountStart = 0, frameStampCountEnd = 0;
|
|
std::vector<long long>frameVec;
|
|
//0106 断连提示
|
|
void _stdcall ExceptionCB(unsigned int nMesgType, void* pUser) {
|
|
mv_Status = "disconnect";
|
|
qDebug() << "[Error] " << " LMI camera disconnects!";
|
|
}
|
|
|
|
#pragma region LMI
|
|
|
|
// Data callback function
|
|
// This function is called from a separate thread spawned by the GoSDK library.
|
|
// Processing within this function should be minimal.
|
|
kStatus kCall onData(void* ctx, void* sys, void* dataset)
|
|
{
|
|
double time_Start = (double)clock();
|
|
unsigned int i, j;
|
|
Mat heightMat1, heightMat2;//高度图
|
|
Mat luminanceMat1, luminanceMat2;//灰度图
|
|
//DataContext* context = (DataContext*)ctx;
|
|
|
|
std::vector<cv::Mat> picVec;
|
|
//qDebug() << "GoDataSet_Count(dataset):" << GoDataSet_Count(dataset);
|
|
for (i = 0; i < GoDataSet_Count(dataset); ++i)
|
|
{
|
|
GoDataMsg dataObj = GoDataSet_At(dataset, i);
|
|
auto type = GoDataMsg_Type(dataObj);
|
|
switch (type)
|
|
{
|
|
//case GO_DATA_MESSAGE_TYPE_STAMP:
|
|
//{
|
|
// GoStampMsg stampMsg = (GoStampMsg)dataObj;
|
|
// // 遍历每个时间戳
|
|
|
|
// for (kSize j = 0; j < GoStampMsg_Count(stampMsg); ++j)
|
|
// {
|
|
// GoStamp* stamp = GoStampMsg_At(stampMsg, j);
|
|
// // 打印触发时间戳
|
|
// if(stamp->frameIndex != lastFrameId)//进行到下一帧
|
|
// {
|
|
//
|
|
// qDebug() << "lastFrameId:" << lastFrameId
|
|
// << "frameStampCountStart:" << frameStampCountStart
|
|
// << "ms frameStampCountEnd:" << frameStampCountEnd << "ms";
|
|
// QString frameList;
|
|
// for (auto value : frameVec)
|
|
// frameList.append(QString::number(value)+",");
|
|
// frameList.append(QString::number(frameVec.size()));
|
|
// qDebug() << "frameVec:" << frameList;
|
|
// frameStampCountStart = stamp->timestamp / 1024;
|
|
|
|
// qDebug() << "Start reciving Frame ID: " << stamp->frameIndex
|
|
// << ", Timestamp: " << stamp->timestamp / 1024 << " ms"
|
|
// << ", Encoder: " << stamp->encoder;
|
|
// //<< ",encoderAtZ:" << stamp->encoderAtZ
|
|
// //<< ", status: " << stamp->status
|
|
// //<< ",id:" << stamp->id
|
|
// //<< ", reserved32u: " << stamp->reserved32u
|
|
// //<< ",reserved64u:" << stamp->reserved64u
|
|
// //<< ", status: " << stamp->status
|
|
// //<< ",ptpTime:" << stamp->ptpTime
|
|
// //<< std::endl;
|
|
// lastFrameId = stamp->frameIndex;
|
|
// frameStampCountStart = stamp->timestamp / 1024;
|
|
// frameVec.push_back(stamp->timestamp / 1024);
|
|
// }
|
|
// else
|
|
// {
|
|
// frameStampCountEnd = stamp->timestamp / 1024;
|
|
// frameVec.push_back(stamp->timestamp / 1024);
|
|
// }
|
|
// }
|
|
// //frameStart = true;
|
|
//
|
|
// break;
|
|
//}
|
|
case GO_DATA_MESSAGE_TYPE_UNIFORM_SURFACE:
|
|
{
|
|
recivingImage = true;
|
|
MyTiff temp;
|
|
GoUniformSurfaceMsg surfaceMsg = dataObj;
|
|
double XResolution = NM_TO_MM(GoSurfaceMsg_XResolution(surfaceMsg));
|
|
double YResolution = NM_TO_MM(GoSurfaceMsg_YResolution(surfaceMsg));
|
|
double ZResolution = NM_TO_MM(GoSurfaceMsg_ZResolution(surfaceMsg));
|
|
double XOffset = UM_TO_MM(GoSurfaceMsg_XOffset(surfaceMsg));
|
|
double YOffset = UM_TO_MM(GoSurfaceMsg_YOffset(surfaceMsg));
|
|
double ZOffset = UM_TO_MM(GoSurfaceMsg_ZOffset(surfaceMsg));
|
|
|
|
int Width = GoSurfaceMsg_Width(surfaceMsg);
|
|
int Height = GoSurfaceMsg_Length(surfaceMsg);
|
|
|
|
temp.Tiff = new unsigned short[Width * Height * sizeof(unsigned short)];
|
|
temp.w = Width;
|
|
temp.h = Height;
|
|
for (int rowIdx = 0; rowIdx < Height; rowIdx++)
|
|
{
|
|
k16s* data = GoSurfaceMsg_RowAt(surfaceMsg, rowIdx);
|
|
for (int colIdx = 0; colIdx < Width; colIdx++)
|
|
{
|
|
unsigned short val = data[colIdx] - 0x8000;
|
|
//unsigned short val = data[colIdx];
|
|
//unsigned short val = ZOffset + ZResolution * data[colIdx];
|
|
temp.Tiff[rowIdx * Width + colIdx] = val;
|
|
}
|
|
}
|
|
|
|
heightMat1 = cv::Mat(temp.h, temp.w, CV_16U, temp.Tiff);
|
|
heightMat1.copyTo(heightMat2);
|
|
|
|
m_lmiheightFlag = true;
|
|
delete[]temp.Tiff;
|
|
temp.Tiff = nullptr;
|
|
/*cv::Mat dst;
|
|
SYSTEMTIME t;
|
|
std::vector<int> params{ (int)cv::IMWRITE_TIFF_RESUNIT,2, (int)cv::IMWRITE_TIFF_COMPRESSION,1,(int)cv::IMWRITE_TIFF_XDPI,300,(int)cv::IMWRITE_TIFF_YDPI,300 };
|
|
GetLocalTime(&t);
|
|
std::string filename = std::to_string(t.wHour) + "-" + std::to_string(t.wMinute) + "-" + std::to_string(t.wSecond) + "-" + std::to_string(t.wMilliseconds) + ".tiff";
|
|
cv::imwrite(filename, img, params);*/
|
|
}
|
|
break;
|
|
case GO_DATA_MESSAGE_TYPE_SURFACE_INTENSITY:
|
|
{
|
|
recivingImage = true;
|
|
MyBmp temp;
|
|
GoSurfaceIntensityMsg surfaceIntMsg = dataObj;
|
|
//double ZResolution = NM_TO_MM(GoSectionMsg_ZResolution(surfaceIntMsg));
|
|
double XResolution = NM_TO_MM(GoSurfaceIntensityMsg_XResolution(surfaceIntMsg));
|
|
double YResolution = NM_TO_MM(GoSurfaceIntensityMsg_YResolution(surfaceIntMsg));
|
|
double XOffset = UM_TO_MM(GoSurfaceIntensityMsg_XOffset(surfaceIntMsg));
|
|
double YOffset = UM_TO_MM(GoSurfaceIntensityMsg_YOffset(surfaceIntMsg));
|
|
|
|
int Width = GoSurfaceIntensityMsg_Width(surfaceIntMsg);
|
|
int Height = GoSurfaceIntensityMsg_Length(surfaceIntMsg);
|
|
|
|
temp.Bmp = new unsigned char[Width * Height * sizeof(unsigned char)];
|
|
temp.w = Width;
|
|
temp.h = Height;
|
|
|
|
for (int rowIdx = 0; rowIdx < Height; rowIdx++)
|
|
{
|
|
k8u* data = GoSurfaceIntensityMsg_RowAt(surfaceIntMsg, rowIdx);
|
|
for (int colIdx = 0; colIdx < Width; colIdx++)
|
|
{
|
|
temp.Bmp[rowIdx * Width + colIdx] = data[colIdx];
|
|
}
|
|
}
|
|
|
|
luminanceMat1 = cv::Mat(temp.h, temp.w, CV_8UC1, temp.Bmp);
|
|
luminanceMat1.copyTo(luminanceMat2);
|
|
m_lmiluminanceFlag = true;
|
|
delete[]temp.Bmp;
|
|
temp.Bmp = nullptr;
|
|
//SYSTEMTIME t;
|
|
//GetLocalTime(&t);
|
|
//std::string filename = std::to_string(t.wHour) + "-" + std::to_string(t.wMinute) + "-" + std::to_string(t.wSecond) + "-" + std::to_string(t.wMilliseconds) + ".bmp";
|
|
//cv::imwrite(filename, img);
|
|
}
|
|
break;
|
|
}
|
|
//DataSetCollection.push_back(dataObj);
|
|
}
|
|
if (recivingImage == false)
|
|
return true;
|
|
recivingImage = false;
|
|
frameStart = false;
|
|
if (!luminanceMat2.empty())
|
|
{
|
|
picVec.push_back(luminanceMat2);
|
|
}
|
|
else
|
|
{
|
|
luminanceMat2 = imread("C://empty.png");
|
|
picVec.push_back(luminanceMat2);
|
|
}
|
|
if( !heightMat2.empty())
|
|
{
|
|
picVec.push_back(heightMat2);
|
|
}
|
|
else
|
|
{
|
|
heightMat2 = imread("C://empty2.png");
|
|
picVec.push_back(heightMat2);
|
|
}
|
|
m_mutex->lock();
|
|
if(picVec.size() > 0)
|
|
queuePic.push_front(picVec);
|
|
m_mutex->unlock();
|
|
heightMat1.release();
|
|
heightMat2.release();
|
|
luminanceMat1.release();
|
|
luminanceMat2.release();
|
|
picVec.clear();
|
|
GoDestroy(dataset);
|
|
double time_End = (double)clock();
|
|
qDebug() << "[INFO] " "image pic:" << picIndex++ << " srcImage Exist,Time:" << (time_End - time_Start) << "ms" << "--CameraName:" << id;
|
|
|
|
return kOK;
|
|
}
|
|
|
|
|
|
bool InitLmi(k32u ID)
|
|
{
|
|
|
|
//return true;//edit
|
|
kStatus status;
|
|
if ((status = GoSdk_Construct(&api)) != kOK)
|
|
{
|
|
//printf("Error: GoSdk_Construct:%d\n", status);
|
|
qDebug() << "[Error] " << "Error: GoSdk_Construct:%d\n", status;
|
|
return false;
|
|
}
|
|
|
|
if ((status = GoSystem_Construct(&system1, kNULL)) != kOK)
|
|
{
|
|
//printf("Error: GoSystem_Construct:%d\n", status);
|
|
qDebug() << "[Error] " << "Error: GoSystem_Construct:%d\n", status;
|
|
return false;
|
|
}
|
|
|
|
if ((status = GoSystem_FindSensorById(system1, ID, &sensor)) != kOK)
|
|
{
|
|
//printf("Error: GoSystem_FindSensor:%d\n", status);
|
|
qDebug() << "[Error] " << "Error: GoSystem_FindSensor:%d\n", status;
|
|
return false;
|
|
}
|
|
|
|
/*GoSystem_Disconnect(system1);*/
|
|
// create connection to GoSystem object
|
|
if ((status = GoSensor_Connect(sensor)) != kOK)
|
|
{
|
|
//ExceptionCB(status, NULL);
|
|
qDebug() << "[Error] " << "Error: GoSensor_Connect:%d\n", status;
|
|
return false;
|
|
}
|
|
|
|
// enable sensor data channel
|
|
if ((status = GoSystem_EnableData(system1, kTRUE)) != kOK)
|
|
{
|
|
//printf("Error: GoSensor_EnableData:%d\n", status);
|
|
qDebug() << "[Error] "<< "Error: GoSensor_EnableData:%d\n", status;
|
|
return false;
|
|
}
|
|
|
|
// set data handler to receive data asynchronously
|
|
if ((status = GoSystem_SetDataHandler(system1, onData, contextPointer)) != kOK)
|
|
{
|
|
//printf("Error: GoSystem_SetDataHandler:%d\n", status);
|
|
qDebug() << "[Error] " << "Error: GoSystem_SetDataHandler:%d\n", status;
|
|
return false;
|
|
}
|
|
|
|
if ((status = GoSystem_SetDataCapacity(system1, kSIZE_MAX)) != kOK)
|
|
{
|
|
qDebug() << "[Error] " << "Error: GoSystem_SetDataCapacity:%d\n", status;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool StartLmi()
|
|
{
|
|
|
|
kStatus status;
|
|
if ((status = GoSystem_Start(system1)) != kOK)
|
|
{
|
|
//printf("Error: GoSystem_Stop:%d\n", status);
|
|
qDebug() << "[Error] " << "Failed to start LMI ";
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool StopLmi()
|
|
{
|
|
kStatus status;
|
|
if ((status = GoSystem_Stop(system1)) != kOK)
|
|
{
|
|
qDebug() << "[Error] " << "Failed to stop LMI ";
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void CloseLmi()
|
|
{
|
|
// destroy handles
|
|
GoDestroy(system1);
|
|
GoDestroy(api);
|
|
}
|
|
|
|
#pragma endregion
|
|
|
|
|
|
Hd_3DCameraLMI_module::Hd_3DCameraLMI_module()
|
|
{
|
|
m_mutex = new QMutex();
|
|
}
|
|
Hd_3DCameraLMI_module::~Hd_3DCameraLMI_module()
|
|
{
|
|
ExceptionCB(-1, NULL);
|
|
}
|
|
void Hd_3DCameraLMI_module::genBlackImg(std::vector<cv::Mat>& vecPic)
|
|
{
|
|
cv::Mat blank_image1(1000, 1000, CV_8UC1, cv::Scalar(0, 0, 0));
|
|
cv::Mat blank_image2(1000, 1000, CV_8UC1, cv::Scalar(0, 0, 0));
|
|
vector<cv::Mat> dstvec;
|
|
dstvec.push_back(blank_image1);
|
|
dstvec.push_back(blank_image2);
|
|
vecPic.assign(dstvec.begin(), dstvec.end());
|
|
}
|
|
bool Hd_3DCameraLMI_module::sendData(std::vector<cv::Mat>& vecPic, QByteArray &data )
|
|
{
|
|
|
|
if (data == "connect")
|
|
{
|
|
data = mv_Status;
|
|
return true;
|
|
}
|
|
|
|
|
|
m_mutex->lock();
|
|
if (queuePic.empty())
|
|
{
|
|
//qDebug() << " [Error] " << " srcImage is null";
|
|
genBlackImg(vecPic);
|
|
m_mutex->unlock();
|
|
return false;
|
|
}
|
|
vector<cv::Mat> dstvec = queuePic.dequeue();
|
|
queuePic.clear();
|
|
m_mutex->unlock();
|
|
vecPic.assign(dstvec.begin(), dstvec.end());
|
|
return true;
|
|
|
|
}
|
|
|
|
bool Hd_3DCameraLMI_module::reciveData(std::vector<cv::Mat> &, QByteArray &qbyte)
|
|
{
|
|
string reciveid = qbyte.toStdString();
|
|
|
|
id = atoi(reciveid.c_str());
|
|
|
|
return true;
|
|
}
|
|
|
|
bool Hd_3DCameraLMI_module::initParas(QByteArray &)
|
|
{
|
|
bool flag = InitLmi(id); //3D初始化
|
|
m_status = flag ? OK : UnInit;
|
|
if (!flag)
|
|
{
|
|
qDebug() << "[Error] " << "Failed to Init LMI,id:"<< id;
|
|
return false;
|
|
}
|
|
|
|
flag = StartLmi();
|
|
m_status == flag ? Grab : OK;
|
|
|
|
if (!flag)
|
|
{
|
|
qDebug() << "[Error] " << " Failed to start LMI";
|
|
return false;
|
|
}
|
|
|
|
qDebug() << "[INFO] " << " success to start LMI,id:" << id;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool Hd_3DCameraLMI_module::run()
|
|
{
|
|
|
|
bool result = false;
|
|
//qDebug() << "[info]" << __FUNCTION__ << " line:" << __LINE__ << " run start" << id;
|
|
try
|
|
{
|
|
//方法1 超时机制
|
|
// queuePic.clear();
|
|
//int timeOut = 0;
|
|
//while (timeOut < 500)//ms
|
|
//{
|
|
// m_mutex->lock();
|
|
// if (queuePic.size() > 0)
|
|
// {
|
|
// result = true;
|
|
// m_mutex->unlock();
|
|
// return true;
|
|
// }
|
|
// m_mutex->unlock();
|
|
// timeOut ++;
|
|
// QThread::msleep(1);//修改了睡眠1ms
|
|
//}
|
|
|
|
//方法2 在下一次触发时强制给上一次触发补黑图
|
|
m_mutex->lock();
|
|
if (triggerIng == true)//仍然在处理上一次触发
|
|
{
|
|
std::vector<cv::Mat>imgs;
|
|
genBlackImg(imgs);
|
|
queuePic.enqueue(imgs);
|
|
m_mutex->unlock();
|
|
int cnt = 0;
|
|
while (queuePic.size() > 0 || cnt < 100)
|
|
{
|
|
QThread::msleep(1);
|
|
cnt++;
|
|
if (cnt > 10)
|
|
qDebug() << "insert black img 2 long";
|
|
}
|
|
}
|
|
else
|
|
{
|
|
triggerIng = true;
|
|
m_mutex->unlock();
|
|
}
|
|
//强制给上一次触发补黑图 结束
|
|
while (true)
|
|
{
|
|
m_mutex->lock();
|
|
if (queuePic.size() > 0)
|
|
{
|
|
result = true;
|
|
triggerIng = false;
|
|
m_mutex->unlock();
|
|
return true;
|
|
}
|
|
m_mutex->unlock();
|
|
QThread::msleep(1);
|
|
}
|
|
}
|
|
catch (QString ev)
|
|
{
|
|
qDebug() << "[Error] " << " ev:" << ev ;
|
|
}
|
|
//qDebug() << "[info]"<< __FUNCTION _ << " line:" << __LINE__ << " run end" ;
|
|
return result;
|
|
}
|
|
|
|
Hd_3DCameraLMI_module *create()
|
|
{
|
|
qDebug() << "[INFO] " << "Hd_MvCamera_module create success!" ;
|
|
return new Hd_3DCameraLMI_module;
|
|
}
|
|
|
|
void destory(Hd_3DCameraLMI_module *ptr)
|
|
{
|
|
if (ptr)
|
|
{
|
|
delete ptr;
|
|
ptr = nullptr;
|
|
}
|
|
qDebug() << "[INFO] " << "Hd_MvCamera_module destory success!";
|
|
}
|