1360 lines
47 KiB
C++
1360 lines
47 KiB
C++
#include "camera.h"
|
||
|
||
#include <QTimer>
|
||
|
||
// 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 = 4096;
|
||
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 = 4096;
|
||
static MIL_INT BufSizeY1 = 512;
|
||
unsigned char* m_AvsBuffer1 = (unsigned char*)malloc(BufSizeX1 * BufSizeY1 * 3);
|
||
static int FuncCount1 = 1;
|
||
|
||
int SaveImg_Flag;
|
||
ONNXRunner runner;
|
||
|
||
|
||
std::map<std::string, int> params;
|
||
int dual_cam_offset_y = 0; // 双相机之间的上下偏移值
|
||
int widthBlocks = 20; // 输出的喷阀通道数
|
||
int heightBlocks = 512; // 输出的Mask高度
|
||
int sizeThreshold = 20; // 转化为喷阀的每块要求像素个数
|
||
int expansionRaidus = 1; // 获取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 file_delay;
|
||
extern int file_encoder;
|
||
extern int file_valve;
|
||
|
||
Timer CallBackTimer0;
|
||
Timer CallBackTimer1;
|
||
|
||
// ONNXRunner runner;
|
||
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;
|
||
}
|
||
|
||
#if(GlobalDebug && DebugDetection)
|
||
// #define SAVE_PATH_resize MIL_TEXT ("C:\\Users\\Pc\\Desktop\\cotton_double2//resize.png")
|
||
#define SAVE_PATH_flip MIL_TEXT ("C:\\Users\\Pc\\Desktop\\cotton_double2//flip.png")
|
||
#define SAVE_PATH_raw MIL_TEXT ("C:\\Users\\Pc\\Desktop\\cotton_double2//raw.png")
|
||
#define SAVE_PATH_result MIL_TEXT ("C:\\Users\\Pc\\Desktop\\cotton_double2//result.png")
|
||
#endif
|
||
|
||
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);
|
||
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);
|
||
}
|
||
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);
|
||
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 = 1;
|
||
recognitionData.image = img;
|
||
g_img_Queue[1]->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);
|
||
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);
|
||
#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("CallBack1: DL detection finished within 20ms");
|
||
timer_detection_time.restart();
|
||
#endif
|
||
// 将深度学习的检测结果转换为 OpenCV Mat
|
||
dl_mask = generateMaskFromMatImage(dl_data.image, widthBlocks, heightBlocks, sizeThreshold);
|
||
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
|
||
// 更新持久化存储
|
||
#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 && DebugDetection)
|
||
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);
|
||
|
||
// 结果后处理Post Process
|
||
// 将每个结果左右扩展，扩展半径默认为1
|
||
auto mask_expaned = expandMaskHorizontally(merged_mask, expansionRaidus);
|
||
// 将结果的左右补充上0，让物体大小符合要求
|
||
PadColumns(mask_expaned, padLeft, padRight, 0);
|
||
//将mask扩展到合适发送的大小
|
||
std::vector<std::vector<uint8_t>> mask_Total = expandArray(mask_expaned,64);
|
||
|
||
// save masks
|
||
#if(GlobalDebug && DebugDetection)
|
||
VectorToImg(mask_1,"C:/Users/Pc/Desktop/img/mask" + std::to_string(FuncCount1) + ".bmp");
|
||
VectorToImg(mask_1,"C:/Users/Pc/Desktop/img/mask_ignored" + std::to_string(FuncCount1) + ".bmp");
|
||
VectorToImg(mask_Total,"C:/Users/Pc/Desktop/img/mask_expended" + std::to_string(FuncCount1) + ".bmp");
|
||
#endif
|
||
// 发送到下位机
|
||
bool result_Low = get_valve_data(mask_Total);
|
||
if(!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;
|
||
}
|
||
|
||
// Optimized LabProcess function
|
||
void lab_process_raw(const MIL_ID& inputImage, MIL_ID& outputImageLab, const std::map<std::string, int>& params,
|
||
const std::vector<std::string>& color_vector)
|
||
{
|
||
MIL_ID MilLabImage = M_NULL, MilLChannel = M_NULL, MilAChannel = M_NULL, MilBChannel = M_NULL;
|
||
MIL_ID lab_result=M_NULL;
|
||
|
||
int denoising = params.at("lab_denoising");
|
||
|
||
// Check number of bands
|
||
MIL_INT NumBands = 0;
|
||
MbufInquire(inputImage, M_SIZE_BAND, &NumBands);
|
||
if (NumBands != 3)
|
||
{
|
||
printf("输入图像不是 3 通道图像，请提供彩色图像。\n");
|
||
return;
|
||
}
|
||
|
||
// Inquire image properties once
|
||
MIL_ID MilSystem = MbufInquire(inputImage, M_OWNER_SYSTEM, M_NULL);
|
||
MIL_INT SizeX = MbufInquire(inputImage, M_SIZE_X, M_NULL);
|
||
MIL_INT SizeY = MbufInquire(inputImage, M_SIZE_Y, M_NULL);
|
||
|
||
// Allocate buffer for Lab image
|
||
MbufAllocColor(MilSystem, 3, SizeX, SizeY, 8 + M_UNSIGNED, M_IMAGE + M_PROC, &MilLabImage);
|
||
|
||
// Convert image from sRGB to Lab
|
||
MimConvert(inputImage, MilLabImage, M_SRGB_TO_LAB);
|
||
|
||
// Create child buffers for L, a, b channels
|
||
MbufChildColor(MilLabImage, 0, &MilLChannel);
|
||
MbufChildColor(MilLabImage, 1, &MilAChannel);
|
||
MbufChildColor(MilLabImage, 2, &MilBChannel);
|
||
|
||
// Allocate output image as 1-bit image
|
||
MbufAlloc2d(MilSystem, SizeX, SizeY, 8 + M_UNSIGNED, M_IMAGE + M_PROC, &outputImageLab);
|
||
MbufClear(outputImageLab, 0); // Initialize to 0
|
||
|
||
// Pre-allocate binary buffers as 1-bit images
|
||
MIL_ID MilBinaryL = M_NULL, MilBinaryA = M_NULL, MilBinaryB = M_NULL, MilResultLab = M_NULL;
|
||
MbufAlloc2d(MilSystem, SizeX, SizeY, 8 + M_UNSIGNED, M_IMAGE + M_PROC, &MilBinaryL);
|
||
MbufAlloc2d(MilSystem, SizeX, SizeY, 8 + M_UNSIGNED, M_IMAGE + M_PROC, &MilBinaryA);
|
||
MbufAlloc2d(MilSystem, SizeX, SizeY, 8 + M_UNSIGNED, M_IMAGE + M_PROC, &MilBinaryB);
|
||
MbufAlloc2d(MilSystem, SizeX, SizeY, 8 + M_UNSIGNED, M_IMAGE + M_PROC, &MilResultLab);
|
||
MbufAlloc2d(MilSystem, SizeX, SizeY, 8 + M_UNSIGNED, M_IMAGE + M_PROC, &lab_result);
|
||
|
||
|
||
// Iterate over colors
|
||
// 遍历颜色
|
||
for (const auto& color : color_vector) {
|
||
// 构建参数键
|
||
std::string L_min_key = color + "_L_min";
|
||
std::string L_max_key = color + "_L_max";
|
||
std::string a_min_key = color + "_a_min";
|
||
std::string a_max_key = color + "_a_max";
|
||
std::string b_min_key = color + "_b_min";
|
||
std::string b_max_key = color + "_b_max";
|
||
|
||
// 获取参数值
|
||
int L_min = params.at(L_min_key);
|
||
int L_max = params.at(L_max_key);
|
||
int a_min = params.at(a_min_key);
|
||
int a_max = params.at(a_max_key);
|
||
int b_min = params.at(b_min_key);
|
||
int b_max = params.at(b_max_key);
|
||
std::vector<int> lab_min_ps = {L_min, a_min, b_min};
|
||
std::vector<int> lab_max_ps = {L_max, a_max, b_max};
|
||
|
||
std::vector<int> lab_min_cv = psLabToOpenCVLab(lab_min_ps);
|
||
std::vector<int> lab_max_cv = psLabToOpenCVLab(lab_max_ps);
|
||
|
||
L_min = lab_min_cv[0];
|
||
L_max = lab_max_cv[0];
|
||
a_min = lab_min_cv[1];
|
||
a_max = lab_max_cv[1];
|
||
b_min = lab_min_cv[2];
|
||
b_max = lab_max_cv[2];
|
||
|
||
// 对每个通道进行二值化
|
||
MimBinarize(MilLChannel, MilBinaryL, M_IN_RANGE, L_min, L_max);
|
||
MimBinarize(MilAChannel, MilBinaryA, M_IN_RANGE, a_min, a_max);
|
||
MimBinarize(MilBChannel, MilBinaryB, M_IN_RANGE, b_min, b_max);
|
||
|
||
// 合并阈值结果
|
||
MimArith(MilBinaryL, MilBinaryA, MilResultLab, M_AND);
|
||
MimArith(MilResultLab, MilBinaryB, MilResultLab, M_AND);
|
||
|
||
// 与输出图像合并
|
||
MimArith(lab_result, MilResultLab, lab_result, M_OR);
|
||
|
||
}
|
||
MimClose(lab_result, MilResultLab, denoising, M_BINARY);
|
||
MimOpen(MilResultLab, outputImageLab, denoising, M_BINARY);
|
||
|
||
// Free binary buffers
|
||
MbufFree(MilBinaryL);
|
||
MbufFree(MilBinaryA);
|
||
MbufFree(MilBinaryB);
|
||
MbufFree(MilResultLab);
|
||
|
||
// Free resources
|
||
MbufFree(MilLChannel);
|
||
MbufFree(MilAChannel);
|
||
MbufFree(MilBChannel);
|
||
MbufFree(MilLabImage);
|
||
MbufFree(lab_result);
|
||
}
|
||
|
||
|
||
void lab_process(const MIL_ID& inputImage, MIL_ID& outputImageLab, const std::map<std::string, int>& params) {
|
||
const std::vector<std::string> colors = {"green", "blue", "orange", "black", "red", "purple", "yellow"};
|
||
lab_process_raw(inputImage, outputImageLab, params, colors);
|
||
}
|
||
|
||
vector<int> psLabToOpenCVLab(const vector<int>& lab_ps) {
|
||
int l_ps = lab_ps[0];
|
||
int a_ps = lab_ps[1];
|
||
int b_ps = lab_ps[2];
|
||
|
||
// Conversion formulas
|
||
int l_cv = round((l_ps / 100.0) * 255.0); // Scale L from 0-100 to 0-255
|
||
int a_cv = round(((a_ps + 128.0) / 255.0) * 255.0); // Shift and scale a
|
||
int b_cv = round(((b_ps + 128.0) / 255.0) * 255.0); // Shift and scale b
|
||
|
||
return {l_cv, a_cv, b_cv};
|
||
}
|
||
|
||
vector<int> opencvLabToPsLab(const vector<int>& lab_cv) {
|
||
int l_cv = lab_cv[0];
|
||
int a_cv = lab_cv[1];
|
||
int b_cv = lab_cv[2];
|
||
|
||
// Conversion formulas
|
||
int l_ps = round((l_cv / 255.0) * 100.0); // Scale L from 0-255 to 0-100
|
||
int a_ps = round((a_cv / 255.0) * 255.0 - 128.0); // Scale and shift a
|
||
int b_ps = round((b_cv / 255.0) * 255.0 - 128.0); // Scale and shift b
|
||
|
||
return {l_ps, a_ps, b_ps};
|
||
}
|
||
|
||
void hsv_process(const MIL_ID& inputImage, MIL_ID& outputImageHSV, const std::map<std::string, int>& params)
|
||
{
|
||
MIL_ID MilHSVImage = M_NULL, MilHChannel = M_NULL, MilSChannel = M_NULL, MilVChannel = M_NULL;
|
||
MIL_ID hsv_result = M_NULL;
|
||
MIL_ID hsv_denoising = M_NULL;
|
||
int saturationThreshold = params.at("saturation_threshold");
|
||
int denoising = params.at("saturation_denoising");
|
||
|
||
// 检查输入图像的通道数
|
||
MIL_INT NumBands = 0;
|
||
MbufInquire(inputImage, M_SIZE_BAND, &NumBands);
|
||
if (NumBands != 3)
|
||
{
|
||
printf("输入图像不是 3 通道图像，请提供彩色图像。\n");
|
||
return;
|
||
}
|
||
|
||
// 分配用于存储 HSV 图像的缓冲区
|
||
MbufAllocColor(MbufInquire(inputImage, M_OWNER_SYSTEM, M_NULL), 3,
|
||
MbufInquire(inputImage, M_SIZE_X, M_NULL),
|
||
MbufInquire(inputImage, M_SIZE_Y, M_NULL),
|
||
8 + M_UNSIGNED,
|
||
M_IMAGE + M_PROC + M_DISP,
|
||
&MilHSVImage);
|
||
|
||
// 将图像从 sRGB 转换到 HSV
|
||
MimConvert(inputImage, MilHSVImage, M_RGB_TO_HSV);
|
||
|
||
// 创建 HSV 通道的子缓冲区
|
||
MbufChildColor(MilHSVImage, 0, &MilHChannel);
|
||
MbufChildColor(MilHSVImage, 1, &MilSChannel);
|
||
MbufChildColor(MilHSVImage, 2, &MilVChannel);
|
||
|
||
// 分配输出图像缓冲区
|
||
MbufAlloc2d(MilSystem, MbufInquire(inputImage, M_SIZE_X, M_NULL),
|
||
MbufInquire(inputImage, M_SIZE_Y, M_NULL), 8 + M_UNSIGNED,
|
||
M_IMAGE + M_PROC + M_DISP, &hsv_result);
|
||
MbufAlloc2d(MilSystem, MbufInquire(inputImage, M_SIZE_X, M_NULL),
|
||
MbufInquire(inputImage, M_SIZE_Y, M_NULL), 8 + M_UNSIGNED,
|
||
M_IMAGE + M_PROC + M_DISP, &hsv_denoising);
|
||
MbufAlloc2d(MilSystem, MbufInquire(inputImage, M_SIZE_X, M_NULL),
|
||
MbufInquire(inputImage, M_SIZE_Y, M_NULL), 8 + M_UNSIGNED,
|
||
M_IMAGE + M_PROC + M_DISP, &outputImageHSV);
|
||
|
||
// 对 S 通道进行阈值分割
|
||
MimBinarize(MilSChannel, hsv_result, M_GREATER,
|
||
saturationThreshold, M_NULL);
|
||
|
||
MimClose(hsv_result, hsv_denoising, denoising, M_BINARY);
|
||
MimOpen(hsv_denoising, outputImageHSV, denoising, M_BINARY);
|
||
|
||
// 释放资源
|
||
MbufFree(MilHChannel);
|
||
MbufFree(MilSChannel);
|
||
MbufFree(MilVChannel);
|
||
MbufFree(MilHSVImage);
|
||
MbufFree(hsv_result);
|
||
MbufFree(hsv_denoising);
|
||
}
|
||
|
||
|
||
void high_sat_detect(const MIL_ID& inputImage, MIL_ID& outputImage, const std::map<std::string, int>& params) {
|
||
MIL_ID output_hsv=M_NULL, output_lab=M_NULL;
|
||
|
||
hsv_process(inputImage, output_hsv, params);
|
||
lab_process(inputImage, output_lab, params);
|
||
|
||
MbufAlloc2d(MilSystem, MbufInquire(inputImage, M_SIZE_X, M_NULL),
|
||
MbufInquire(inputImage, M_SIZE_Y, M_NULL), 8 + M_UNSIGNED,
|
||
M_IMAGE + M_PROC, &outputImage);
|
||
|
||
|
||
// 合并 Lab 和 HSV 的结果（取“或”运算）
|
||
MimArith(output_hsv, output_lab, outputImage, M_OR);
|
||
MbufFree(output_lab);
|
||
MbufFree(output_hsv);
|
||
}
|
||
|
||
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;
|
||
}
|
||
}
|
||
|
||
std::vector<std::vector<uint8_t>> generateMaskFromImage(const MIL_ID& inputImage, int widthBlocks, int heightBlocks, int thresholds= 10) {
|
||
// 读取图像
|
||
// cv::Mat image = cv::imread(imagePath, cv::IMREAD_GRAYSCALE);
|
||
|
||
// // 检查图像是否成功读取
|
||
// if (image.empty()) {
|
||
// std::cerr << "无法加载图像，请检查路径是否正确: " << imagePath << std::endl;
|
||
// exit(EXIT_FAILURE);
|
||
// }
|
||
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 widthBlocks, int heightBlocks, int thresholds= 10) {
|
||
|
||
// 确保图像是二值化的-*
|
||
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(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[i][j] = true;
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
return mask;
|
||
}
|
||
|
||
|
||
bool iniColor()
|
||
{
|
||
read_params_from_file((getConfigDirectory()+"/color_range_config.txt").toStdString(), params);
|
||
return 1;
|
||
}
|
||
|
||
|
||
bool iniOnnx()
|
||
{
|
||
|
||
std::string modelPath = (getConfigDirectory() + "/dimo_369_640.onnx").toStdString();
|
||
|
||
runner.load(modelPath);
|
||
|
||
// cv::Mat mask;
|
||
// std::vector<Detection> result = runner.predict(image);
|
||
// mask = runner.postProcess(result, image);
|
||
// std::string savepath = "C:/Users/admin/Desktop/config/suspect_mask.png";
|
||
// cv::imwrite(savepath, mask);
|
||
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 0;
|
||
}
|
||
#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 0 ;
|
||
}
|
||
|
||
|
||
|
||
// 计算 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'));
|
||
|
||
// QString delay_time = QString("%1").arg(file_delay, 0, 16); // 去掉补0的部分
|
||
// QString divide_parameter = QString("%1").arg(file_encoder, 0, 16);
|
||
// QString sv_parameter = QString("%1").arg(file_valve, 0, 16);
|
||
|
||
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);
|
||
} 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);
|
||
} 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;
|
||
|
||
qDebug()<<"相机参数"<<divide_byte.data();
|
||
qDebug()<<"阀门参数"<<sv_byte.data();
|
||
|
||
if (lower_machine->isWritable()) {
|
||
lower_machine->write((const char*)valve_divide_buf, len_sv + 8);
|
||
} else {
|
||
cout << "Error: Unable to write to lower machine for valve parameter." << endl;
|
||
}
|
||
delete[] valve_divide_buf;
|
||
|
||
|
||
|
||
return 1;
|
||
}
|
||
|
||
bool DestoryLowMac()
|
||
{
|
||
MdigProcess(MilDigitizer0, MilGrabBufferList0, 20, M_STOP, M_DEFAULT,ProcessingFunction0, M_NULL);
|
||
MdigProcess(MilDigitizer1, MilGrabBufferList1, 20, M_STOP, M_DEFAULT,ProcessingFunction1, M_NULL);
|
||
|
||
// 构建停止命令
|
||
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;
|
||
// ui->lab_lowermachine_isconnect->setStyleSheet("QLabel{background-color: rgb(237, 212, 0);}"); // 显示连接失败
|
||
// ui->lab_lowermachine_isconnect->repaint(); // 强制刷新UI
|
||
}
|
||
|
||
delete[] mask_buf; // 释放内存
|
||
|
||
return true;
|
||
}
|
||
|
||
|
||
void Start_camera()
|
||
{
|
||
MdigProcess(MilDigitizer0, MilGrabBufferList0, 20, M_START, M_DEFAULT,ProcessingFunction0, M_NULL);
|
||
MdigProcess(MilDigitizer1, MilGrabBufferList1, 20, M_START, M_DEFAULT,ProcessingFunction1, M_NULL);
|
||
|
||
// 发送开始命令
|
||
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;
|
||
}
|