1390 lines
47 KiB
C++
1390 lines
47 KiB
C++
#include "camera.h"
|
||
|
||
|
||
// Debug Options
|
||
#define GlobalDebug 0 // 全局是否允许打印Debug信息（打印会拖慢处理时间）
|
||
#define DebugDetection 0 // 注意开启这个编译选项会导致图片存储, 处理时间会很慢
|
||
#define DebugDetectionTime 0 // 是否打印处理时间
|
||
#define DebugLowerMacCOM 0 // 是否打印和下位机通讯的相关信息
|
||
|
||
camera::camera() {}
|
||
|
||
QTcpServer* server_to_lowermachine = nullptr;
|
||
QTcpSocket* lower_machine = nullptr;
|
||
bool volatile is_running = false;
|
||
|
||
MIL_ID MilApplication;
|
||
MIL_ID MilSystem;
|
||
|
||
static MIL_ID MilDigitizer0;
|
||
static MIL_ID MilImage0;
|
||
static MIL_ID MilImage_Color0;
|
||
static MIL_ID ModifiedBufferId0;
|
||
static MIL_ID MilGrabBufferList0[20] = {0};
|
||
static MIL_INT BufSizeX0 = 2048;
|
||
static MIL_INT BufSizeY0 = 512;
|
||
unsigned char* m_AvsBuffer0 = (unsigned char*)malloc(BufSizeX0 * BufSizeY0 * 3);
|
||
static int FuncCount0 = 1;
|
||
|
||
static MIL_ID MilDigitizer1;
|
||
static MIL_ID MilImage1;
|
||
static MIL_ID MilImage_Color1;
|
||
static MIL_ID ModifiedBufferId1;
|
||
static MIL_ID MilGrabBufferList1[20] = {0};
|
||
static MIL_INT BufSizeX1 = 2048;
|
||
static MIL_INT BufSizeY1 = 512;
|
||
unsigned char* m_AvsBuffer1 = (unsigned char*)malloc(BufSizeX1 * BufSizeY1 * 3);
|
||
static int FuncCount1 = 1;
|
||
|
||
int SaveImg_Flag;
|
||
ONNXRunner runner;
|
||
|
||
|
||
int dual_cam_offset_y = 0; // 双相机之间的上下偏移值
|
||
int widthBlocks = 20; // 输出的喷阀通道数
|
||
int heightBlocks = 512; // 输出的Mask高度
|
||
int padLeft = 1; // 左侧在结果Mask上补0
|
||
int padRight = 1; // 右侧在结果Mask上补0
|
||
int rowRange = 1; // 结果维持行数（至少为1）
|
||
int ignoreSide = 0; // 左右两侧的忽略的喷阀通道数量
|
||
int skipLeftCols = 0; // 生成mask时跳过左侧的像素数量
|
||
int skipRightCols = 0; // 生成mask时跳过右侧的像素数量
|
||
|
||
static std::vector<std::vector<uint8_t>> mask_0(0);
|
||
static std::vector<std::vector<uint8_t>> mask_1(0);
|
||
|
||
static std::vector<std::vector<uint8_t>> tail_0(0);
|
||
static std::vector<std::vector<uint8_t>> tail_1(0);
|
||
|
||
|
||
uint8_t temp_buf[512 * 64] = {0};
|
||
|
||
// 总体参数
|
||
extern int lowmac_sm; //吹气量
|
||
extern int file_encoder;
|
||
extern int file_valve;
|
||
|
||
// 颜色模块参数
|
||
extern int sizeThreshold; // 转化为喷阀的每块要求像素个数
|
||
extern int expansionRaidus; // 获取mask后进行左右位置的扩展
|
||
extern int file_delay;
|
||
|
||
// 偏振模块参数
|
||
extern int lowmac_dp; //偏振延迟时间
|
||
extern int lowmac_ts; //模板匹配阈值
|
||
extern int lowmac_sg; //偏振绿色通道大小阈值
|
||
extern int lowmac_td; //偏振红色通道差值
|
||
|
||
Timer CallBackTimer0;
|
||
Timer CallBackTimer1;
|
||
|
||
|
||
bool iniCamera()
|
||
{
|
||
//分配application
|
||
MappAlloc(M_DEFAULT, &MilApplication);
|
||
|
||
//分配system
|
||
MsysAlloc(M_DEFAULT, M_SYSTEM_RADIENTEVCL, M_DEV0, M_DEFAULT, &MilSystem);
|
||
|
||
//分配相机 digitier
|
||
QString config_dir = getConfigDirectory();
|
||
MdigAlloc(MilSystem,M_DEV0,(config_dir + "/1.dcf").toStdWString(), M_DEFAULT,&MilDigitizer0);
|
||
MdigAlloc(MilSystem,M_DEV1,(config_dir + "/2.dcf").toStdWString(), M_DEFAULT,&MilDigitizer1);
|
||
|
||
//给MilImage分配空间
|
||
MbufAllocColor(MilSystem,3,BufSizeX0,BufSizeY0,8 + M_UNSIGNED,M_IMAGE + M_GRAB + M_PROC,&MilImage0);
|
||
MbufAllocColor(MilSystem,3,BufSizeX1,BufSizeY1,8 + M_UNSIGNED,M_IMAGE + M_GRAB + M_PROC,&MilImage1);
|
||
MbufAllocColor(MilSystem,3,BufSizeX0,BufSizeY0,8 + M_UNSIGNED,M_IMAGE + M_GRAB + M_PROC,&MilImage_Color0);
|
||
MbufAllocColor(MilSystem,3,BufSizeX1,BufSizeY1,8 + M_UNSIGNED,M_IMAGE + M_GRAB + M_PROC,&MilImage_Color1);
|
||
|
||
|
||
//给每一个bufferlist分配空间
|
||
for (int i = 0; i < 20; i++)
|
||
{
|
||
// 系统 3维度 宽 高 8位无符号 图像数据 对象
|
||
MbufAllocColor(MilSystem,3,BufSizeX0,BufSizeY0,8 + M_UNSIGNED,M_IMAGE + M_GRAB + M_PROC,&MilGrabBufferList0[i]);
|
||
if (MilGrabBufferList0[i])
|
||
{
|
||
MbufClear(MilGrabBufferList0[i], 0xFF);
|
||
}
|
||
else
|
||
{
|
||
break;
|
||
}
|
||
}
|
||
|
||
for (int i = 0; i < 20; i++)
|
||
{
|
||
// 系统 3维度 宽 高 8位无符号 图像数据 对象
|
||
MbufAllocColor(MilSystem,3,BufSizeX1,BufSizeY1,8 + M_UNSIGNED,M_IMAGE + M_GRAB + M_PROC,&MilGrabBufferList1[i]);
|
||
if (MilGrabBufferList1[i])
|
||
{
|
||
MbufClear(MilGrabBufferList1[i], 0xFF);
|
||
}
|
||
else
|
||
{
|
||
break;
|
||
}
|
||
}
|
||
#if(GlobalDebug)
|
||
qDebug()<<"ready";
|
||
#endif
|
||
|
||
return 1;
|
||
}
|
||
|
||
|
||
MIL_INT ProcessingFunction0(MIL_INT HookType, MIL_ID HookId, void *HookDataPtr)
|
||
{
|
||
#if(GlobalDebug)
|
||
qDebug()<<"CallBack1";
|
||
Timer call_back_timer0;
|
||
call_back_timer0.restart();
|
||
#endif
|
||
|
||
int camera_id = 0;
|
||
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
Timer timer_detection_time;
|
||
timer_detection_time.restart();
|
||
#endif
|
||
|
||
MdigGetHookInfo(HookId, M_MODIFIED_BUFFER + M_BUFFER_ID, &ModifiedBufferId0);
|
||
{
|
||
QMutexLocker locker(&gDispPicMutex0);
|
||
gDispCurrentPicId0 = ModifiedBufferId0;
|
||
}
|
||
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
timer_detection_time.printElapsedTime("CallBack1: Send mil id for detection");
|
||
timer_detection_time.restart();
|
||
#endif
|
||
|
||
if (SaveImg_Flag)
|
||
{
|
||
// 拷贝存图图像
|
||
MbufCopy(ModifiedBufferId0, MilImage0);
|
||
cv::Mat img = ImageUtils::mil2Mat(MilImage0);
|
||
// 将图像数据推入存储队列
|
||
ImageData data;
|
||
data.camera_id = 0;
|
||
data.image = img.clone(); // 确保图像数据被复制
|
||
g_storageQueue.enqueue(data);
|
||
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
qDebug() << "CallBack1: push image to storage queue";
|
||
#endif
|
||
}
|
||
|
||
// 拷贝艳丽色检测图像
|
||
MbufCopy(ModifiedBufferId0, MilImage_Color0);
|
||
MIL_UNIQUE_BUF_ID MimResizedestination = MbufAllocColor(MilSystem, 3, 1024, 512, 8+M_UNSIGNED, M_IMAGE+M_PROC+M_DISP, M_UNIQUE_ID);
|
||
MbufClear(MimResizedestination, M_COLOR_BLACK);
|
||
MimResize(MilImage_Color0, MimResizedestination, 0.5, 1 , M_DEFAULT);
|
||
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
timer_detection_time.printElapsedTime("CallBack1: High sat detection resize");
|
||
timer_detection_time.restart();
|
||
#endif
|
||
|
||
// 图片镜像翻转
|
||
MIL_UNIQUE_BUF_ID MimFlipDedtination = MbufClone(MimResizedestination, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_UNIQUE_ID);
|
||
MbufClear(MimFlipDedtination, M_COLOR_BLACK);
|
||
MimFlip(MimResizedestination, MimFlipDedtination, M_FLIP_HORIZONTAL, M_DEFAULT);
|
||
cv::Mat img = ImageUtils::mil2Mat(MimFlipDedtination);
|
||
|
||
// 将图像推入识别队列
|
||
if(g_dl_enable[camera_id])
|
||
{
|
||
ImageData recognitionData;
|
||
recognitionData.camera_id = camera_id;
|
||
recognitionData.image = img;
|
||
g_img_Queue[camera_id]->enqueue(recognitionData);
|
||
}
|
||
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
timer_detection_time.printElapsedTime("CallBack1: High sat detection mirror and push to DL");
|
||
timer_detection_time.restart();
|
||
#endif
|
||
|
||
// 艳丽检测mask
|
||
Mat matrox_mat;
|
||
std::vector<std::vector<uint8_t>> matrox_mask;
|
||
if(g_traditional_enable[camera_id])
|
||
{
|
||
// 使用 std::async 将 high_sat_detect 封装为异步任务
|
||
auto future = std::async(std::launch::async, [&]() {
|
||
MIL_ID detection_result_id;
|
||
high_sat_detect(MimFlipDedtination, detection_result_id, params,colors);
|
||
matrox_mat = ImageUtils::mil2Mat(detection_result_id);
|
||
MbufFree(detection_result_id);
|
||
});
|
||
|
||
// 等待最多20毫秒
|
||
if(future.wait_for(std::chrono::milliseconds(20)) == std::future_status::ready)
|
||
{
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
timer_detection_time.printElapsedTime("CallBack1: High sat detection finish");
|
||
timer_detection_time.restart();
|
||
#endif
|
||
// 任务在指定时间内完成，可以继续使用 detection_result1 \
|
||
// 将 Matrox 的检测结果转换为 二维vector
|
||
matrox_mask = generateMaskFromMatImage(matrox_mat, widthBlocks, heightBlocks, sizeThreshold,
|
||
params["screen_left_" + std::to_string(camera_id)],
|
||
params["screen_right_" + std::to_string(camera_id)]);
|
||
}
|
||
else
|
||
{
|
||
// 任务超时，处理超时情况
|
||
qWarning() << "high_sat_detect 超时, 未在指定时间内完成。";
|
||
// 创建一个大小为 widthBlocks x heightBlocks 的 matrox_mask
|
||
matrox_mask = std::vector<std::vector<uint8_t>>(heightBlocks, std::vector<uint8_t>(widthBlocks, 0));
|
||
}
|
||
}
|
||
else
|
||
{
|
||
matrox_mask = std::vector<std::vector<uint8_t>>(heightBlocks, std::vector<uint8_t>(widthBlocks, 0));
|
||
}
|
||
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
timer_detection_time.printElapsedTime("CallBack1: High sat detection to mask");
|
||
timer_detection_time.restart();
|
||
#endif
|
||
// 获取深度学习的检测结果并进行合并
|
||
std::vector<std::vector<uint8_t>> merged_mask;
|
||
std::vector<std::vector<uint8_t>> dl_mask;
|
||
ImageData dl_data;
|
||
|
||
if (g_dl_enable[camera_id])
|
||
{
|
||
// 使用 std::async 将深度学习检测封装为异步任务
|
||
auto future_dl = std::async(std::launch::async, [&]() -> bool {
|
||
// 尝试从队列中获取检测结果，假设 dequeue 支持超时
|
||
// 如果 dequeue 不支持超时，请确保它不会无限期阻塞
|
||
bool success = g_result_Queue[camera_id]->dequeue(dl_data);
|
||
return success;
|
||
});
|
||
|
||
// 等待最多20毫秒
|
||
if (future_dl.wait_for(std::chrono::milliseconds(20)) == std::future_status::ready && future_dl.get())
|
||
{
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
timer_detection_time.printElapsedTime("CallBack1: DL detection finished within 20ms");
|
||
timer_detection_time.restart();
|
||
#endif
|
||
// 将深度学习的检测结果转换为 OpenCV Mat
|
||
dl_mask = generateMaskFromMatImage(dl_data.image, widthBlocks, heightBlocks, sizeThreshold,
|
||
params["screen_left_" + std::to_string(camera_id)],
|
||
params["screen_right_" + std::to_string(camera_id)]);
|
||
merged_mask = ImageUtils::mergeMasks(dl_mask, matrox_mask);
|
||
}
|
||
else
|
||
{
|
||
// 任务超时或未成功获取结果，使用 Matrox 的检测结果
|
||
qWarning() << "DL detection 超时或未成功获取结果 for camera" << camera_id;
|
||
dl_mask = matrox_mask;
|
||
merged_mask = matrox_mask;
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
timer_detection_time.printElapsedTime("CallBack1: DL detection error or timeout to mask");
|
||
timer_detection_time.restart();
|
||
#endif
|
||
}
|
||
}
|
||
else
|
||
{
|
||
dl_mask = matrox_mask;
|
||
merged_mask = matrox_mask;
|
||
}
|
||
|
||
// Update the current Img MIl id
|
||
// 更新持久化存储
|
||
// {
|
||
// QMutexLocker locker(&g_detection_result[camera_id].mutex);
|
||
// g_detection_result[camera_id].dl_mask = dl_mask;
|
||
// g_detection_result[camera_id].traditional_mask = matrox_mask;
|
||
// }
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
timer_detection_time.printElapsedTime("CallBack1: Push result to g_detection_result");
|
||
timer_detection_time.restart();
|
||
#endif
|
||
mask_0 = merged_mask;
|
||
detection_ready.release();
|
||
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
call_back_timer0.printElapsedTime("CallBack1: Total time spent: ");
|
||
#endif
|
||
return 0;
|
||
}
|
||
|
||
|
||
MIL_INT ProcessingFunction1(MIL_INT HookType, MIL_ID HookId, void *HookDataPtr)
|
||
{
|
||
#if(GlobalDebug)
|
||
qDebug()<<"CallBack2";
|
||
Timer call_back_timer1;
|
||
call_back_timer1.restart();
|
||
#endif
|
||
int camera_id = 1;
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
Timer timer_detection_time;
|
||
timer_detection_time.restart();
|
||
#endif
|
||
MdigGetHookInfo(HookId, M_MODIFIED_BUFFER + M_BUFFER_ID, &ModifiedBufferId1);
|
||
// Update the current Img MIl id for display
|
||
{
|
||
QMutexLocker locker(&gDispPicMutex1);
|
||
gDispCurrentPicId1 = ModifiedBufferId1;
|
||
}
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
timer_detection_time.printElapsedTime("CallBack2: Send mil id for detection");
|
||
timer_detection_time.restart();
|
||
#endif
|
||
|
||
// 将图像数据推入存储队列
|
||
if (SaveImg_Flag)
|
||
{
|
||
// 转回OpenCV图像
|
||
MbufCopy(ModifiedBufferId1, MilImage1);
|
||
cv::Mat img = ImageUtils::mil2Mat(MilImage1);
|
||
ImageData data;
|
||
data.camera_id = 1;
|
||
data.image = img.clone(); // 确保图像数据被复制
|
||
g_storageQueue.enqueue(data);
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
qDebug() << "CallBack2: push image to storage queue";
|
||
#endif
|
||
}
|
||
|
||
//拷贝艳丽色检测图像
|
||
MbufCopy(ModifiedBufferId1,MilImage_Color1);
|
||
MIL_UNIQUE_BUF_ID MimResizedestination = MbufAllocColor(MilSystem, 3, 1024, 512, 8+M_UNSIGNED, M_IMAGE+M_PROC+M_DISP, M_UNIQUE_ID);
|
||
MbufClear(MimResizedestination, M_COLOR_BLACK);
|
||
MimResize(MilImage_Color1, MimResizedestination, 0.5, 1 , M_DEFAULT);
|
||
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
timer_detection_time.printElapsedTime("CallBack2: High sat detection resize");
|
||
timer_detection_time.restart();
|
||
#endif
|
||
|
||
if(g_dl_enable[camera_id])
|
||
{
|
||
// 将图像推入识别队列
|
||
cv::Mat img = ImageUtils::mil2Mat(MimResizedestination);
|
||
ImageData recognitionData;
|
||
recognitionData.camera_id = camera_id;
|
||
recognitionData.image = img;
|
||
g_img_Queue[camera_id]->enqueue(recognitionData);
|
||
}
|
||
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
timer_detection_time.printElapsedTime("CallBack2: Without mirror and push to DL");
|
||
timer_detection_time.restart();
|
||
#endif
|
||
|
||
// 艳丽检测mask
|
||
Mat matrox_mat;
|
||
std::vector<std::vector<uint8_t>> matrox_mask;
|
||
if(g_traditional_enable[camera_id])
|
||
{
|
||
// 使用 std::async 将 high_sat_detect 封装为异步任务
|
||
auto future = std::async(std::launch::async, [&]() {
|
||
MIL_ID detection_result_id;
|
||
high_sat_detect(MimResizedestination, detection_result_id, params,colors);
|
||
matrox_mat = ImageUtils::mil2Mat(detection_result_id);
|
||
MbufFree(detection_result_id);
|
||
});
|
||
|
||
// 等待最多20毫秒
|
||
if(future.wait_for(std::chrono::milliseconds(20)) == std::future_status::ready)
|
||
{
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
timer_detection_time.printElapsedTime("CallBack2: High sat detection finish");
|
||
timer_detection_time.restart();
|
||
#endif
|
||
// 任务在20ms内完成，可以继续使用 detection_result1 \
|
||
// 将 Matrox 的检测结果转换为 二维vector
|
||
matrox_mask = generateMaskFromMatImage(matrox_mat, widthBlocks, heightBlocks, sizeThreshold,
|
||
params["screen_left_" + std::to_string(camera_id)],
|
||
params["screen_right_" + std::to_string(camera_id)]);
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
timer_detection_time.printElapsedTime("CallBack2: High sat detection to mask");
|
||
timer_detection_time.restart();
|
||
#endif
|
||
}
|
||
else
|
||
{
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
timer_detection_time.printElapsedTime("CallBack2: High sat detection timeout");
|
||
timer_detection_time.restart();
|
||
#endif
|
||
// 任务超时，处理超时情况
|
||
qWarning() << "high_sat_detect 超时,未在20ms内完成。";
|
||
// 创建一个大小为 widthBlocks x heightBlocks 的 matrox_mask
|
||
matrox_mask = std::vector<std::vector<uint8_t>>(heightBlocks, std::vector<uint8_t>(widthBlocks, 0));
|
||
}
|
||
}
|
||
else
|
||
{
|
||
matrox_mask = std::vector<std::vector<uint8_t>>(heightBlocks, std::vector<uint8_t>(widthBlocks, 0));
|
||
}
|
||
|
||
// 获取深度学习的检测结果并进行合并
|
||
std::vector<std::vector<uint8_t>> merged_mask;
|
||
std::vector<std::vector<uint8_t>> dl_mask;
|
||
ImageData dl_data;
|
||
|
||
if (g_dl_enable[camera_id])
|
||
{
|
||
// 使用 std::async 将深度学习检测封装为异步任务
|
||
auto future_dl = std::async(std::launch::async, [&]() -> bool {
|
||
// 尝试从队列中获取检测结果，假设 dequeue 支持超时
|
||
// 如果 dequeue 不支持超时，请确保它不会无限期阻塞
|
||
bool success = g_result_Queue[camera_id]->dequeue(dl_data);
|
||
return success;
|
||
});
|
||
|
||
// 等待最多20毫秒
|
||
if (future_dl.wait_for(std::chrono::milliseconds(20)) == std::future_status::ready && future_dl.get())
|
||
{
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
timer_detection_time.printElapsedTime("CallBack2: DL detection finished within 20ms");
|
||
timer_detection_time.restart();
|
||
#endif
|
||
// 将深度学习的检测结果转换为 OpenCV Mat
|
||
dl_mask = generateMaskFromMatImage(dl_data.image, widthBlocks, heightBlocks, sizeThreshold,
|
||
params["screen_left_" + std::to_string(camera_id)],
|
||
params["screen_right_" + std::to_string(camera_id)]);
|
||
merged_mask = ImageUtils::mergeMasks(dl_mask, matrox_mask);
|
||
}
|
||
else
|
||
{
|
||
// 任务超时或未成功获取结果，使用 Matrox 的检测结果
|
||
qWarning() << "DL detection 超时或未成功获取结果 for camera" << camera_id;
|
||
dl_mask = matrox_mask;
|
||
merged_mask = matrox_mask;
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
timer_detection_time.printElapsedTime("CallBack2: DL detection error or timeout to mask");
|
||
timer_detection_time.restart();
|
||
#endif
|
||
}
|
||
}
|
||
else
|
||
{
|
||
dl_mask = matrox_mask;
|
||
merged_mask = matrox_mask;
|
||
}
|
||
|
||
|
||
// Update the current Img MIl id
|
||
// 更新持久化存储
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
timer_detection_time.printElapsedTime("CallBack2: Push result to g_detection_result");
|
||
timer_detection_time.restart();
|
||
#endif
|
||
mask_1 = merged_mask;
|
||
|
||
#if(GlobalDebug && DebufgDetection)
|
||
MbufSave(SAVE_PATH_flip,MimFlipDedtination);
|
||
// MbufSave(SAVE_PATH_resize,MimResizedestination);
|
||
MbufSave(SAVE_PATH_raw,MilImage_Color0);
|
||
MbufSave(SAVE_PATH_result,detection_result0);
|
||
#endif
|
||
// 等待另一个算法线程执行结束
|
||
detection_ready.acquire();
|
||
|
||
vector<vector<uint8_t>> mask_tail;
|
||
|
||
merged_mask = ImageUtils::mergeMasks(mask_0, mask_1, dual_cam_offset_y);
|
||
std::tie(merged_mask, mask_tail) = ImageUtils::extractROI(merged_mask, 0, 0, widthBlocks, heightBlocks);
|
||
|
||
#if(GlobalDebug && DebugDetection)
|
||
int row_count = 0;
|
||
std::cout << "<<<<<<<<<<<<<<<<<<<<<Mask_0<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<" << std::endl;
|
||
for(auto row: mask_0)
|
||
{ std::cout << row_count << " ";
|
||
for (auto element: row)
|
||
{
|
||
std::cout << int(element) << " ";
|
||
}
|
||
std::cout << std::endl;
|
||
row_count ++;
|
||
if(row_count >3)
|
||
break;
|
||
}
|
||
std::cout << ">>>>>>>>>>>>>>>>>>>>Mask_0>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>" << std:: endl;
|
||
row_count = 0;
|
||
std::cout << "<<<<<<<<<<<<<<<<<<<<<Mask_1<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<" << std::endl;
|
||
for(auto row: mask_1)
|
||
{ std::cout << row_count << " ";
|
||
for (auto element: row)
|
||
{
|
||
|
||
std::cout << int(element) << " ";
|
||
}
|
||
std::cout << std::endl;
|
||
row_count ++;
|
||
if(row_count >3)
|
||
break;
|
||
}
|
||
std::cout << ">>>>>>>>>>>>>>>>>>>>Mask_1>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>" << std:: endl;
|
||
row_count = 0;
|
||
std::cout << "<<<<<<<<<<<<<<<<<<<<<merged_mask<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<" << std::endl;
|
||
for(auto row: merged_mask)
|
||
{ std::cout << row_count << " ";
|
||
for (auto element: row)
|
||
{
|
||
|
||
std::cout << int(element) << " ";
|
||
}
|
||
std::cout << std::endl;
|
||
row_count ++;
|
||
if(row_count >3)
|
||
break;
|
||
}
|
||
std::cout << ">>>>>>>>>>>>>>>>>>>>merged_mask>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>" << std:: endl;
|
||
#endif
|
||
|
||
// 结果后处理Post Process
|
||
// 将每个结果左右扩展，扩展半径默认为1
|
||
auto mask_expaned = expandMaskHorizontally(merged_mask, expansionRaidus);
|
||
// 将结果的左右补充上0，让物体大小符合要求
|
||
PadColumns(mask_expaned, padLeft, padRight, 0);
|
||
|
||
max_valves_together_limit(mask_expaned, params["max_valves_together"]);
|
||
|
||
//将mask扩展到合适发送的大小
|
||
std::vector<std::vector<uint8_t>> mask_Total = expandArray(mask_expaned,64);
|
||
|
||
if(!g_lower_machine_connected.load())
|
||
{
|
||
qWarning() << "下位机未连接";
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
call_back_timer1.printElapsedTime("CallBack2: Total time spent: ");
|
||
#endif
|
||
return 0;
|
||
}
|
||
// 发送到下位机
|
||
bool result_Low = get_valve_data(mask_Total);
|
||
if(!result_Low)
|
||
{
|
||
g_lower_machine_connected.store(result_Low);
|
||
qWarning() << "下位机发送失败";
|
||
}
|
||
|
||
|
||
#if(GlobalDebug && DebugDetectionTime)
|
||
call_back_timer1.printElapsedTime("CallBack2: Total time spent: ");
|
||
#endif
|
||
return 0;
|
||
}
|
||
|
||
|
||
bool DestoryCamera()
|
||
{
|
||
for (int i = 0; i < 20; i++)
|
||
{
|
||
MbufFree(MilGrabBufferList0[i]);
|
||
}
|
||
|
||
for (int i = 0; i < 20; i++)
|
||
{
|
||
MbufFree(MilGrabBufferList1[i]);
|
||
}
|
||
|
||
MbufFree(MilImage0);
|
||
MbufFree(MilImage1);
|
||
|
||
MbufFree(MilImage_Color0);
|
||
MbufFree(MilImage_Color1);
|
||
|
||
MdigFree(MilDigitizer0);
|
||
MdigFree(MilDigitizer1);
|
||
|
||
MsysFree(MilSystem);
|
||
MappFree(MilApplication);
|
||
|
||
return 1;
|
||
}
|
||
|
||
|
||
void read_params_from_file(const std::string& filename, std::map<std::string, int>& params) {
|
||
std::ifstream infile(filename);
|
||
if (!infile) {
|
||
std::cerr << "无法打开文件: " << filename << std::endl;
|
||
return;
|
||
}
|
||
|
||
std::string line;
|
||
while (std::getline(infile, line)) {
|
||
// 去除行首和行尾的空白字符
|
||
line.erase(0, line.find_first_not_of(" \t\r\n"));
|
||
line.erase(line.find_last_not_of(" \t\r\n") + 1);
|
||
|
||
// 跳过空行和注释行
|
||
if (line.empty() || line[0] == '#')
|
||
continue;
|
||
|
||
// 查找等号的位置
|
||
size_t pos = line.find('=');
|
||
if (pos == std::string::npos)
|
||
continue; // 如果没有等号，跳过该行
|
||
|
||
// 分割键和值，并去除空白字符
|
||
std::string key = line.substr(0, pos);
|
||
std::string value_str = line.substr(pos + 1);
|
||
key.erase(0, key.find_first_not_of(" \t"));
|
||
key.erase(key.find_last_not_of(" \t") + 1);
|
||
value_str.erase(0, value_str.find_first_not_of(" \t"));
|
||
value_str.erase(value_str.find_last_not_of(" \t") + 1);
|
||
|
||
// 将字符串转换为整数
|
||
int value;
|
||
std::istringstream iss(value_str);
|
||
if (!(iss >> value)) {
|
||
std::cerr << "键 " << key << " 的值无效: " << value_str << std::endl;
|
||
continue;
|
||
}
|
||
|
||
// 将键值对添加到参数映射中
|
||
params[key] = value;
|
||
}
|
||
}
|
||
|
||
|
||
void read_color(const std::string& filename, std::vector<std::string>& colors) {
|
||
std::ifstream file(filename);
|
||
if (!file.is_open()) {
|
||
std::cerr << "Error: Unable to open file " << filename << std::endl;
|
||
return;
|
||
}
|
||
|
||
std::vector<std::string> new_colors;
|
||
std::string color;
|
||
while (std::getline(file, color)) {
|
||
if (!color.empty()) {
|
||
new_colors.push_back(color);
|
||
}
|
||
}
|
||
|
||
if (!new_colors.empty()) {
|
||
colors = std::move(new_colors); // Replace the existing colors
|
||
}
|
||
else {
|
||
std::cerr << "Warning: File is empty or contains no valid data." << std::endl;
|
||
}
|
||
|
||
file.close();
|
||
}
|
||
|
||
|
||
std::vector<std::vector<uint8_t>> generateMaskFromImage(const MIL_ID& inputImage, int widthBlocks, int heightBlocks, int thresholds= 10) {
|
||
cv::Mat image=ImageUtils::mil2Mat(inputImage);
|
||
// 确保图像是二值化的
|
||
cv::threshold(image, image, 128, 255, cv::THRESH_BINARY);
|
||
|
||
// 获取图像的宽度和高度
|
||
int imageWidth = image.cols;
|
||
int imageHeight = image.rows;
|
||
|
||
// 计算每个块的宽度和高度
|
||
int blockWidth = imageWidth / widthBlocks;
|
||
int blockHeight = imageHeight / heightBlocks;
|
||
|
||
// 创建掩膜矩阵
|
||
std::vector<std::vector<uint8_t>> mask_1(heightBlocks, std::vector<uint8_t>(widthBlocks, false));
|
||
|
||
// 遍历每个块并统计白色像素点的数量
|
||
for (int i = 0; i < heightBlocks; ++i) {
|
||
for (int j = 0; j < widthBlocks; ++j) {
|
||
// 计算块的起始和结束位置
|
||
int x_start = j * blockWidth;
|
||
int y_start = i * blockHeight;
|
||
int x_end = (j == widthBlocks - 1) ? imageWidth : (j + 1) * blockWidth;
|
||
int y_end = (i == heightBlocks - 1) ? imageHeight : (i + 1) * blockHeight;
|
||
|
||
// 提取当前块
|
||
cv::Mat block = image(cv::Rect(x_start, y_start, x_end - x_start, y_end - y_start));
|
||
|
||
// 统计块中白色像素的数量
|
||
int whitePixelCount = cv::countNonZero(block);
|
||
|
||
// 如果白色像素数大于阈值，将该块标记为 true
|
||
if (whitePixelCount > thresholds) {
|
||
mask_1[i][j] = true;
|
||
}
|
||
}
|
||
}
|
||
|
||
return mask_1;
|
||
|
||
}
|
||
|
||
std::vector<std::vector<uint8_t>> generateMaskFromMatImage(
|
||
const cv::Mat& image,
|
||
int outputWidth,
|
||
int outputHeight,
|
||
int thresholds,
|
||
int skipLeftCols,
|
||
int skipRightCols) {
|
||
|
||
// Ensure the image is binary
|
||
cv::threshold(image, image, 128, 255, cv::THRESH_BINARY);
|
||
|
||
int imageWidth = image.cols;
|
||
int imageHeight = image.rows;
|
||
|
||
// Adjust image width by excluding skipLeftCols and skipRightCols
|
||
int effectiveWidth = imageWidth - skipLeftCols - skipRightCols;
|
||
if (effectiveWidth <= 0) {
|
||
throw std::invalid_argument("Invalid column skip values. Effective width is less than or equal to zero.");
|
||
}
|
||
|
||
int blockWidth = effectiveWidth / outputWidth;
|
||
int blockHeight = imageHeight / outputHeight;
|
||
|
||
vector<vector<uint8_t>> mask(outputHeight, vector<uint8_t>(outputWidth, 0));
|
||
|
||
for (int i = 0; i < outputHeight; ++i) {
|
||
for (int j = 0; j < outputWidth; ++j) {
|
||
int x_start = skipLeftCols + j * blockWidth;
|
||
int y_start = i * blockHeight;
|
||
int x_end = (j == outputWidth - 1) ? (imageWidth - skipRightCols) : (skipLeftCols + (j + 1) * blockWidth);
|
||
int y_end = (i == outputHeight - 1) ? imageHeight : (i + 1) * blockHeight;
|
||
|
||
cv::Mat block = image(cv::Rect(x_start, y_start, x_end - x_start, y_end - y_start));
|
||
|
||
int whitePixelCount = cv::countNonZero(block);
|
||
if (whitePixelCount > sizeThreshold) {
|
||
mask[i][j] = 1;
|
||
}
|
||
}
|
||
}
|
||
return mask;
|
||
}
|
||
|
||
void max_valves_together_limit(std::vector<std::vector<uint8_t>> &mask_expaned, const int row_threshold)
|
||
{
|
||
// 初始化当前回调的1的计数
|
||
long long count_ones_this_time = 0;
|
||
|
||
// 遍历每一行
|
||
for(auto & row : mask_expaned)
|
||
{
|
||
// 计算当前行中1的数量
|
||
long long row_count = 0;
|
||
for(uint8_t val : row)
|
||
{
|
||
row_count += (val == 1);
|
||
}
|
||
|
||
// 检查是否超过阈值
|
||
if(row_count > row_threshold)
|
||
{
|
||
// 如果超过阈值，将整行置为0
|
||
std::fill(row.begin(), row.end(), 0);
|
||
g_camera_error.store(true); // 原子赋值为 true
|
||
}
|
||
else
|
||
{
|
||
count_ones_this_time += row_count;
|
||
g_camera_error.store(false); // 原子赋值为 true
|
||
}
|
||
}
|
||
|
||
// 更新全局的1的计数
|
||
g_valveActionCount += count_ones_this_time;
|
||
|
||
// 如果需要调试打印，可以取消注释以下代码
|
||
// qDebug() << "This callback1 new triggered bits =" << count_ones_this_time
|
||
// << ", total=" << g_valveActionCount;
|
||
}
|
||
|
||
bool iniColor()
|
||
{
|
||
// read_params_from_file((getConfigDirectory()+"/color_range_config.txt").toStdString(), params);
|
||
// loadConfig(getConfigDirectory()+"/color_range_config.txt");
|
||
// read_color((getConfigDirectory()+"/colors.txt").toStdString(), colors);
|
||
return 1;
|
||
}
|
||
|
||
|
||
bool iniOnnx()
|
||
{
|
||
std::string modelPath = (getConfigDirectory() + "/dimo_369_640.onnx").toStdString();
|
||
|
||
runner.load(modelPath);
|
||
return 1;
|
||
}
|
||
|
||
bool iniLowMac()
|
||
{
|
||
/*--- server socket to faban ---*/
|
||
server_to_lowermachine = new QTcpServer();
|
||
#if(GlobalDebug && DebugLowerMacCOM)
|
||
qDebug()<<"1/4. Build Tcp Server";
|
||
#endif
|
||
server_to_lowermachine->listen(QHostAddress::Any, 13452);
|
||
#if(GlobalDebug && DebugLowerMacCOM)
|
||
qDebug()<<"2/4. Bind to Port 13452";
|
||
#endif
|
||
bool is_timeout;
|
||
server_to_lowermachine->waitForNewConnection(5000,&is_timeout);
|
||
#if(GlobalDebug && DebugLowerMacCOM)
|
||
qDebug()<<"3/4. Try to Establish connect";
|
||
#endif
|
||
|
||
if(is_timeout)
|
||
{
|
||
return false;
|
||
}
|
||
#if(GlobalDebug && DebugLowerMacCOM)
|
||
qDebug()<<"4/4. Connection Established no timeout";
|
||
#endif
|
||
|
||
lower_machine = server_to_lowermachine->nextPendingConnection();
|
||
|
||
#if(GlobalDebug && DebugLowerMacCOM)
|
||
qDebug()<<"Lower Machine Connection Suecced";
|
||
#endif
|
||
|
||
if (lower_machine == nullptr || !lower_machine->isWritable()) {
|
||
cout << "Error: Lower machine is not available or writable." << endl;
|
||
return false ;
|
||
}
|
||
return setLowMacParam();
|
||
}
|
||
|
||
bool setLowMacParam(){
|
||
try {
|
||
// 计算 Y = 100000000 / X
|
||
int divide_camera = (file_encoder != 0) ? 100000000 / file_encoder : 0; // 防止除以零的情况
|
||
int divide_valve = (file_valve != 0) ? 100000000 / file_valve : 0; // 防止除以零的情况
|
||
|
||
// 将参数转换为长度为8的字符串，前面补0
|
||
QString delay_time = QString("%1").arg(file_delay, 8, 10, QChar('0'));
|
||
QString divide_parameter = QString("%1").arg(divide_camera, 8, 10, QChar('0'));
|
||
QString sv_parameter = QString("%1").arg(divide_valve, 8, 10, QChar('0'));
|
||
|
||
int len_delay = delay_time.size();
|
||
int len_divide = divide_parameter.size();
|
||
int len_sv = sv_parameter.size();
|
||
|
||
QByteArray delay_byte = delay_time.toLatin1();
|
||
QByteArray divide_byte = divide_parameter.toLatin1();
|
||
QByteArray sv_byte = sv_parameter.toLatin1();
|
||
|
||
// 发送延迟时间
|
||
uint8_t* delay_buf = new uint8_t[len_delay + 8];
|
||
delay_buf[0] = 0xAA;
|
||
delay_buf[1] = 0x00;
|
||
delay_buf[2] = len_delay + 2;
|
||
delay_buf[3] = 's';
|
||
delay_buf[4] = 'd';
|
||
memcpy(delay_buf + 5, delay_byte.data(), len_delay);
|
||
delay_buf[len_delay + 5] = 0xFF;
|
||
delay_buf[len_delay + 6] = 0xFF;
|
||
delay_buf[len_delay + 7] = 0xBB;
|
||
|
||
if (lower_machine->isWritable()) {
|
||
lower_machine->write((const char*)delay_buf, len_delay + 8);
|
||
qDebug() << "彩色相机延迟时间:" << delay_byte.data();
|
||
} else {
|
||
cout << "Error: Unable to write to lower machine for delay parameter." << endl;
|
||
}
|
||
delete[] delay_buf;
|
||
|
||
// 发送相机参数
|
||
uint8_t* divide_buf = new uint8_t[len_divide + 8];
|
||
divide_buf[0] = 0xAA;
|
||
divide_buf[1] = 0x00;
|
||
divide_buf[2] = len_divide + 2;
|
||
divide_buf[3] = 's';
|
||
divide_buf[4] = 'c';
|
||
memcpy(divide_buf + 5, divide_byte.data(), len_divide);
|
||
divide_buf[len_divide + 5] = 0xFF;
|
||
divide_buf[len_divide + 6] = 0xFF;
|
||
divide_buf[len_divide + 7] = 0xBB;
|
||
|
||
if (lower_machine->isWritable()) {
|
||
lower_machine->write((const char*)divide_buf, len_divide + 8);
|
||
qDebug() << "彩色相机分频系数:" << divide_byte.data();
|
||
} else {
|
||
cout << "Error: Unable to write to lower machine for encoder parameter." << endl;
|
||
}
|
||
delete[] divide_buf;
|
||
|
||
// 发送阀门参数
|
||
uint8_t* valve_divide_buf = new uint8_t[len_sv + 8];
|
||
valve_divide_buf[0] = 0xAA;
|
||
valve_divide_buf[1] = 0x00;
|
||
valve_divide_buf[2] = len_sv + 2;
|
||
valve_divide_buf[3] = 's';
|
||
valve_divide_buf[4] = 'v';
|
||
memcpy(valve_divide_buf + 5, sv_byte.data(), len_sv);
|
||
valve_divide_buf[len_sv + 5] = 0xFF;
|
||
valve_divide_buf[len_sv + 6] = 0xFF;
|
||
valve_divide_buf[len_sv + 7] = 0xBB;
|
||
|
||
if (lower_machine->isWritable()) {
|
||
lower_machine->write((const char*)valve_divide_buf, len_sv + 8);
|
||
qDebug() << "阀门分频系数:" << sv_byte.data();
|
||
} else {
|
||
cout << "Error: Unable to write to lower machine for valve parameter." << endl;
|
||
}
|
||
delete[] valve_divide_buf;
|
||
|
||
// 将参数转换为长度为8的字符串，前面补0
|
||
QString dp = QString("%1").arg(lowmac_dp, 8, 10, QChar('0'));
|
||
QString sm = QString("%1").arg(lowmac_sm, 8, 10, QChar('0'));
|
||
QString ts = QString("%1").arg(lowmac_ts, 8, 10, QChar('0'));
|
||
QString sg = QString("%1").arg(lowmac_sg, 8, 10, QChar('0'));
|
||
QString td = QString("%1").arg(lowmac_td, 8, 10, QChar('0'));
|
||
|
||
int len_dp = dp.size();
|
||
int len_sm = sm.size();
|
||
int len_ts = ts.size();
|
||
int len_sg = sg.size();
|
||
int len_td = td.size();
|
||
|
||
QByteArray dp_byte = dp.toLatin1();
|
||
QByteArray sm_byte = sm.toLatin1();
|
||
QByteArray ts_byte = ts.toLatin1();
|
||
QByteArray sg_byte = sg.toLatin1();
|
||
QByteArray td_byte = td.toLatin1();
|
||
|
||
//偏振延迟时间
|
||
uint8_t* dp_buf = new uint8_t[len_dp + 8];
|
||
dp_buf[0] = 0xAA;
|
||
dp_buf[1] = 0x00;
|
||
dp_buf[2] = len_dp + 2;
|
||
dp_buf[3] = 'd';
|
||
dp_buf[4] = 'p';
|
||
memcpy(dp_buf + 5, dp_byte.data(), len_dp);
|
||
dp_buf[len_dp + 5] = 0xFF;
|
||
dp_buf[len_dp + 6] = 0xFF;
|
||
dp_buf[len_dp + 7] = 0xBB;
|
||
|
||
if (lower_machine->isWritable()) {
|
||
lower_machine->write((const char*)dp_buf, len_dp + 8);
|
||
qDebug() << "偏振延迟时间: " << dp_byte.data();
|
||
} else {
|
||
cout << "Error: Unable to write to lower machine for dp parameter." << endl;
|
||
}
|
||
delete[] dp_buf;
|
||
|
||
// 吹气量
|
||
uint8_t* sm_buf = new uint8_t[len_sm + 8];
|
||
sm_buf[0] = 0xAA;
|
||
sm_buf[1] = 0x00;
|
||
sm_buf[2] = len_sm + 2;
|
||
sm_buf[3] = 's';
|
||
sm_buf[4] = 'm';
|
||
memcpy(sm_buf + 5, sm_byte.data(), len_sm);
|
||
sm_buf[len_sm + 5] = 0xFF;
|
||
sm_buf[len_sm + 6] = 0xFF;
|
||
sm_buf[len_sm + 7] = 0xBB;
|
||
|
||
if (lower_machine->isWritable()) {
|
||
lower_machine->write((const char*)sm_buf, len_sm + 8);
|
||
qDebug() << "吹气量: " << sm_byte.data();
|
||
} else {
|
||
cout << "Error: Unable to write to lower machine for sm parameter." << endl;
|
||
}
|
||
delete[] sm_buf;
|
||
|
||
// 模板匹配阈值
|
||
uint8_t* ts_buf = new uint8_t[len_ts + 8];
|
||
ts_buf[0] = 0xAA;
|
||
ts_buf[1] = 0x00;
|
||
ts_buf[2] = len_ts + 2;
|
||
ts_buf[3] = 't';
|
||
ts_buf[4] = 's';
|
||
memcpy(ts_buf + 5, ts_byte.data(), len_ts);
|
||
ts_buf[len_ts + 5] = 0xFF;
|
||
ts_buf[len_ts + 6] = 0xFF;
|
||
ts_buf[len_ts + 7] = 0xBB;
|
||
|
||
if (lower_machine->isWritable()) {
|
||
lower_machine->write((const char*)ts_buf, len_ts + 8);
|
||
qDebug() << "模板匹配阈值: " << ts_byte.data();
|
||
} else {
|
||
cout << "Error: Unable to write to lower machine for ts parameter." << endl;
|
||
}
|
||
delete[] ts_buf;
|
||
|
||
// 偏振绿色通道大小阈值
|
||
uint8_t* sg_buf = new uint8_t[len_sg + 8];
|
||
sg_buf[0] = 0xAA;
|
||
sg_buf[1] = 0x00;
|
||
sg_buf[2] = len_sg + 2;
|
||
sg_buf[3] = 's';
|
||
sg_buf[4] = 'g';
|
||
memcpy(sg_buf + 5, sg_byte.data(), len_sg);
|
||
sg_buf[len_sg + 5] = 0xFF;
|
||
sg_buf[len_sg + 6] = 0xFF;
|
||
sg_buf[len_sg + 7] = 0xBB;
|
||
|
||
if (lower_machine->isWritable()) {
|
||
lower_machine->write((const char*)sg_buf, len_sg + 8);
|
||
qDebug() << "偏振绿色通道大小阈值: " << sg_byte.data();
|
||
} else {
|
||
cout << "Error: Unable to write to lower machine for sg parameter." << endl;
|
||
}
|
||
delete[] sg_buf;
|
||
|
||
// 偏振红色通道差值
|
||
uint8_t* td_buf = new uint8_t[len_td + 8];
|
||
td_buf[0] = 0xAA;
|
||
td_buf[1] = 0x00;
|
||
td_buf[2] = len_td + 2;
|
||
td_buf[3] = 't';
|
||
td_buf[4] = 'd';
|
||
memcpy(td_buf + 5, td_byte.data(), len_td);
|
||
td_buf[len_td + 5] = 0xFF;
|
||
td_buf[len_td + 6] = 0xFF;
|
||
td_buf[len_td + 7] = 0xBB;
|
||
|
||
if (lower_machine->isWritable()) {
|
||
lower_machine->write((const char*)td_buf, len_td + 8);
|
||
qDebug() << "偏振红色通道差值: " << td_byte.data();
|
||
} else {
|
||
cout << "Error: Unable to write to lower machine for td parameter." << endl;
|
||
}
|
||
delete[] td_buf;
|
||
} catch (...) {
|
||
qWarning() << "Set lower machine parameters failed!!!";
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
bool DestoryLowMac()
|
||
{
|
||
// 构建停止命令
|
||
uint8_t stop_command[9] = {0};
|
||
stop_command[0] = 0xAA; // 起始标志
|
||
stop_command[1] = 0x00; // 长度高位
|
||
stop_command[2] = 0x03; // 长度低位
|
||
stop_command[3] = 's'; // 命令类型
|
||
stop_command[4] = 'p'; // 停止命令
|
||
stop_command[5] = 0xFF; // 校验位1
|
||
stop_command[6] = 0xFF; // 校验位2
|
||
stop_command[7] = 0xFF; // 校验位3
|
||
stop_command[8] = 0xBB; // 结束标志
|
||
|
||
// 发送停止命令给下位机
|
||
if(lower_machine != nullptr && lower_machine->isWritable())
|
||
{
|
||
lower_machine->write(reinterpret_cast<const char*>(stop_command), 9);
|
||
lower_machine->flush();
|
||
}
|
||
// 设置运行状态为 false
|
||
is_running = false;
|
||
}
|
||
|
||
bool get_valve_data(std::vector<std::vector<uint8_t>> mask)
|
||
{
|
||
if (mask[0].size() % 8 != 0) {
|
||
std::cerr << "Error: mask 的第 0 行的列数应该为 8 的倍数。" << std::endl;
|
||
return false;
|
||
}
|
||
|
||
uint8_t* mask_buf = new uint8_t[4096 + 8]; // 创建缓冲区，大小为3072 + 8
|
||
mask_buf[0] = 0xAA; // 起始标志
|
||
mask_buf[1] = 0x10; // 高位数据长度 (352 字节 -> 0x0160)
|
||
mask_buf[2] = 0x02; // 低位数据长度
|
||
mask_buf[3] = 'd'; // 命令类型 (发送类型 'd')
|
||
mask_buf[4] = 'a'; // 数据类型 (阀门相关 'a')
|
||
|
||
// 将二维容器中的二值数据转换为字节并存储到 mask_buf 中
|
||
int idx = 5; // 从 mask_buf[5] 开始存储数据
|
||
for (int i = 0; i < 512; i++) // 遍历512行
|
||
{
|
||
for (int j = 0; j < int(64 / 8); j++) // 遍历64列
|
||
{
|
||
uint8_t byte = 0;
|
||
for (int bit_idx = 0; bit_idx < 8; bit_idx++)
|
||
{
|
||
byte |= (mask[i][ j * 8 + bit_idx] & 0x01) << bit_idx; // 每个字节内，低字节优先
|
||
}
|
||
mask_buf[idx++] = byte;
|
||
}
|
||
}
|
||
|
||
mask_buf[4101] = 0xFF; // 校验低位
|
||
mask_buf[4102] = 0xFF; // 校验高位
|
||
mask_buf[4103] = 0xBB; // 结束标志
|
||
#if(GlobalDebug && DebugLowerMacCOM)
|
||
for (int i = 0; i <= 4103; i++)
|
||
{
|
||
// 将 mask_buf[i] 转换为 int 再输出，避免其被当作字符解释
|
||
std::cout << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(mask_buf[i]) << " ";
|
||
}
|
||
std::cout << std::endl;
|
||
#endif
|
||
|
||
// qDebug()<<&mask_buf;
|
||
|
||
// qDebug() << "Sending data to lower machine in binary:";
|
||
// for (int i = 0; i < 8200; ++i)
|
||
// {
|
||
// QString binaryString = QString::number(mask_buf[i], 2).rightJustified(8, '0');
|
||
// qDebug() << QString("Byte %1: %2").arg(i).arg(binaryString);
|
||
// }
|
||
|
||
// 检查与设备的连接状态
|
||
if (lower_machine != nullptr && lower_machine->state() == QAbstractSocket::ConnectedState)
|
||
{
|
||
lower_machine->write((const char*)mask_buf, 4104); // 总共 3080 字节
|
||
lower_machine->flush();
|
||
}
|
||
else
|
||
{
|
||
std::cout << "*** lower machine connect failed! *** " << std::endl;
|
||
delete[] mask_buf; // 释放内存
|
||
return false;
|
||
}
|
||
delete[] mask_buf; // 释放内存
|
||
return true;
|
||
}
|
||
|
||
bool start_grab()
|
||
{
|
||
try
|
||
{
|
||
MdigProcess(MilDigitizer0, MilGrabBufferList0, 20, M_START, M_DEFAULT,ProcessingFunction0, M_NULL);
|
||
MdigProcess(MilDigitizer1, MilGrabBufferList1, 20, M_START, M_DEFAULT,ProcessingFunction1, M_NULL);
|
||
} catch (...) {
|
||
qWarning() << "Start Grabber Failed!!!";
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
bool stop_grab()
|
||
{ try {
|
||
MdigProcess(MilDigitizer0, MilGrabBufferList0, 20, M_STOP, M_DEFAULT,ProcessingFunction0, M_NULL);
|
||
MdigProcess(MilDigitizer1, MilGrabBufferList1, 20, M_STOP, M_DEFAULT,ProcessingFunction1, M_NULL);
|
||
} catch (...) {
|
||
qWarning() << "Stop Grabber Failed!!!";
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
void start_camera()
|
||
{
|
||
start_grab();
|
||
// 发送开始命令
|
||
uint8_t start_command[9] = {0};
|
||
start_command[0] = 0xAA;
|
||
start_command[1] = 0x00;
|
||
start_command[2] = 0x03;
|
||
start_command[3] = 's';
|
||
start_command[4] = 't';
|
||
start_command[5] = 0xFF;
|
||
start_command[6] = 0xFF;
|
||
start_command[7] = 0xFF;
|
||
start_command[8] = 0xBB;
|
||
|
||
if (lower_machine != nullptr && lower_machine->isWritable())
|
||
{
|
||
lower_machine->write((const char*)start_command, 9);
|
||
qDebug()<<"发送相机启动成功";
|
||
}
|
||
else
|
||
{
|
||
cout << "Error: Unable to write to lower machine for start command." << endl;
|
||
}
|
||
}
|
||
|
||
|
||
std::vector<std::vector<uint8_t> > expandArray(const std::vector<std::vector<uint8_t> > &array, int newCols)
|
||
{
|
||
int rows = array.size();
|
||
// 创建一个新的二维数组，初始化为0，列数为 newCols
|
||
std::vector<std::vector<uint8_t>> array_total(rows, std::vector<uint8_t>(newCols, 0));
|
||
|
||
// 将原数组的值复制到新数组的前22列
|
||
for (int i = 0; i < rows; ++i)
|
||
{
|
||
for (int j = 0; j < array[i].size(); ++j)
|
||
{
|
||
array_total[i][j] = array[i][j];
|
||
}
|
||
}
|
||
|
||
return array_total;
|
||
}
|
||
|
||
|
||
vector<vector<uint8_t>> generateMask(
|
||
const MIL_ID& inputImg,
|
||
int outputWidth,
|
||
int outputHeight,
|
||
int sizeThreshold,
|
||
int skipLeftCols,
|
||
int skipRightCols
|
||
) {
|
||
cv::Mat image = ImageUtils::mil2Mat(inputImg);
|
||
|
||
// Ensure the image is binary
|
||
cv::threshold(image, image, 128, 255, cv::THRESH_BINARY);
|
||
|
||
int imageWidth = image.cols;
|
||
int imageHeight = image.rows;
|
||
|
||
// Adjust image width by excluding skipLeftCols and skipRightCols
|
||
int effectiveWidth = imageWidth - skipLeftCols - skipRightCols;
|
||
if (effectiveWidth <= 0) {
|
||
throw std::invalid_argument("Invalid column skip values. Effective width is less than or equal to zero.");
|
||
}
|
||
|
||
int blockWidth = effectiveWidth / outputWidth;
|
||
int blockHeight = imageHeight / outputHeight;
|
||
|
||
vector<vector<uint8_t>> mask(outputHeight, vector<uint8_t>(outputWidth, 0));
|
||
|
||
for (int i = 0; i < outputHeight; ++i) {
|
||
for (int j = 0; j < outputWidth; ++j) {
|
||
int x_start = skipLeftCols + j * blockWidth;
|
||
int y_start = i * blockHeight;
|
||
int x_end = (j == outputWidth - 1) ? (imageWidth - skipRightCols) : (skipLeftCols + (j + 1) * blockWidth);
|
||
int y_end = (i == outputHeight - 1) ? imageHeight : (i + 1) * blockHeight;
|
||
|
||
cv::Mat block = image(cv::Rect(x_start, y_start, x_end - x_start, y_end - y_start));
|
||
|
||
int whitePixelCount = cv::countNonZero(block);
|
||
if (whitePixelCount > sizeThreshold) {
|
||
mask[i][j] = 1;
|
||
}
|
||
}
|
||
}
|
||
|
||
return mask;
|
||
}
|
||
|
||
|
||
pair<vector<vector<uint8_t>>, vector<vector<uint8_t>>> applyRowRangeDelay(
|
||
const vector<vector<uint8_t>>& mask,
|
||
const vector<vector<uint8_t>>& tail,
|
||
int rowRange
|
||
)
|
||
{
|
||
if(rowRange <= 0)
|
||
{
|
||
rowRange = 1;
|
||
qDebug() << "Maintain Row Range Error, forced to 1!!!!!!!!";
|
||
}
|
||
int outputHeight = (int)mask.size();
|
||
int outputWidth = (int)mask[0].size();
|
||
|
||
vector<vector<uint8_t>> mask_after_row_range(outputHeight, vector<uint8_t>(outputWidth, 0));
|
||
vector<vector<uint8_t>> newTail(rowRange, vector<uint8_t>(outputWidth, 0));
|
||
|
||
// 先将旧的 tail 映射到 mask_after_row_range 的顶部几行
|
||
for (int i = 0; i < (int)tail.size(); ++i) {
|
||
for (int j = 0; j < outputWidth; ++j) {
|
||
if (i < outputHeight) {
|
||
mask_after_row_range[i][j] = max(mask_after_row_range[i][j], tail[i][j]);
|
||
}
|
||
}
|
||
}
|
||
|
||
// 对当前 mask 应用 rowRange 的拖影效果
|
||
for (int j = 0; j < outputWidth; ++j) {
|
||
for (int i = 0; i < outputHeight; ++i) {
|
||
if (mask[i][j] == 1) {
|
||
// 从当前行 i 开始，向下扩展 rowRange 行
|
||
int end_line = i + rowRange - 1;
|
||
|
||
// 先处理仍在 mask 范围内的部分
|
||
int inside_mask_end = min(end_line, outputHeight - 1);
|
||
for (int line = i; line <= inside_mask_end; ++line) {
|
||
mask_after_row_range[line][j] = 1;
|
||
}
|
||
|
||
// 超出 mask 范围的行进入 tail
|
||
if (end_line >= outputHeight) {
|
||
// 从 outputHeight 行开始的部分属于 tail
|
||
for (int line = outputHeight; line <= end_line; ++line) {
|
||
int tail_line_idx = line - outputHeight;
|
||
if (tail_line_idx >= 0 && tail_line_idx < (int)newTail.size()) {
|
||
newTail[tail_line_idx][j] = 1;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
return {mask_after_row_range, newTail};
|
||
}
|
||
|
||
|
||
// Updated wrapper function
|
||
pair<vector<vector<uint8_t>>, vector<vector<uint8_t>>> generateMaskWithTail(
|
||
const MIL_ID& inputImg,
|
||
const vector<vector<uint8_t>>& tail,
|
||
int outputWidth,
|
||
int outputHeight,
|
||
int sizeThreshold = 10,
|
||
int rowRange=1,
|
||
int skipLeftCols=10,
|
||
int skipRightCols=10
|
||
) {
|
||
// Generate the mask from the image
|
||
vector<vector<uint8_t>> mask = generateMask(inputImg, outputWidth, outputHeight, sizeThreshold, skipLeftCols,skipRightCols);
|
||
|
||
// Apply rowRange delay
|
||
return applyRowRangeDelay(mask, tail, rowRange);
|
||
}
|
||
|
||
|
||
|
||
void PadColumns(std::vector<std::vector<uint8_t>>& data, int pad_left, int pad_right, uint8_t fill_value = 0)
|
||
{
|
||
// 如果不需要填充，直接返回
|
||
if (pad_left <= 0 && pad_right <= 0) {
|
||
return;
|
||
}
|
||
|
||
for (auto& row : data) {
|
||
// 在左侧插入pad_left个fill_value
|
||
row.insert(row.begin(), pad_left, fill_value);
|
||
// 在右侧插入pad_right个fill_value
|
||
row.insert(row.end(), pad_right, fill_value);
|
||
}
|
||
}
|
||
|
||
|
||
std::vector<std::vector<uint8_t>> expandMaskHorizontally(
|
||
const std::vector<std::vector<uint8_t>>& mask,
|
||
int expansionRadius)
|
||
{
|
||
std::vector<std::vector<uint8_t>> expanded_mask = mask;
|
||
int rows = (int)mask.size();
|
||
if (rows == 0) return expanded_mask;
|
||
int cols = (int)mask[0].size();
|
||
|
||
for (int y = 0; y < rows; y++)
|
||
{
|
||
for (int x = 0; x < cols; x++)
|
||
{
|
||
if (mask[y][x] == 1)
|
||
{
|
||
for (int i = 1; i <= expansionRadius; i++)
|
||
{
|
||
if (x - i >= 0)
|
||
expanded_mask[y][x - i] = 1;
|
||
if (x + i < cols)
|
||
expanded_mask[y][x + i] = 1;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
return expanded_mask;
|
||
}
|