389 lines
12 KiB
C++
389 lines
12 KiB
C++
// camAlgoSW_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
|
|
//
|
|
|
|
#include <iostream>
|
|
#include <fstream>
|
|
#include "..\camAlgoSW\sourceCode\camAlgoSW_Export.h"
|
|
#include <opencv2/opencv.hpp>
|
|
#include <vector>
|
|
#include "..\camAlgoSW\sourceCode\algoGlobals.h"
|
|
|
|
typedef struct
|
|
{
|
|
int nMin; //< 最小值
|
|
int nMax; //< 最大值
|
|
} SWdNLRange;
|
|
|
|
void saveFrontEndData(char* fileName, std::vector<Luma_rgnData>& frontendData)
|
|
{
|
|
if (0 == frontendData.size())
|
|
return;
|
|
|
|
std::ofstream TXTFile(fileName);
|
|
char TXTData[250];
|
|
snprintf(TXTData, sizeof(TXTData), "%3x", (0x800 | 0x10));//MEANSTG_WIN_SIZE);
|
|
TXTFile << TXTData << std::endl;
|
|
|
|
for (int i = 0, i_max = (int)frontendData.size(); i < i_max; i++)
|
|
{
|
|
Luma_rgnData data = frontendData[i];
|
|
snprintf(TXTData, sizeof(TXTData), "%04x %04x %04x %01x %02x", data.WinRdx, data.y, data.Rid, data.Flag, data.PeakRltvRdx);
|
|
TXTFile << TXTData << std::endl;
|
|
for (int j = 0; j < 16; j++)
|
|
{
|
|
snprintf(TXTData, sizeof(TXTData), "%02x", data.data[j]);
|
|
TXTFile << TXTData << std::endl;
|
|
}
|
|
}
|
|
TXTFile.close();
|
|
}
|
|
|
|
void readFrontEndDataFile(char* fileName, std::vector<Luma_rgnData>& frontendData)
|
|
{
|
|
std::ifstream inputFile(fileName);
|
|
std::string linedata;
|
|
|
|
if (inputFile.is_open() == false)
|
|
return;
|
|
|
|
std::getline(inputFile, linedata); //第一行
|
|
|
|
int idx = 0;
|
|
Luma_rgnData a_pk;
|
|
while (std::getline(inputFile, linedata))
|
|
{
|
|
if (idx == 0)
|
|
{
|
|
int WinRdx, y, Rid, Flag, PeakRltvRdx;
|
|
sscanf_s(linedata.c_str(), "%04x %04x %04x %01x %02x", &WinRdx, &y, &Rid, &Flag, &PeakRltvRdx);
|
|
a_pk.WinRdx = WinRdx;
|
|
a_pk.y = y;
|
|
a_pk.Rid = Rid;
|
|
a_pk.Flag = Flag;
|
|
a_pk.PeakRltvRdx = PeakRltvRdx;
|
|
idx++;
|
|
}
|
|
else
|
|
{
|
|
int value;
|
|
sscanf_s(linedata.c_str(), "%02x", &value);
|
|
a_pk.data[idx - 1] = value;
|
|
idx++;
|
|
}
|
|
if (idx == 17)
|
|
{
|
|
frontendData.push_back(a_pk);
|
|
idx = 0;
|
|
}
|
|
}
|
|
inputFile.close();
|
|
return;
|
|
}
|
|
|
|
void saveCenteringData(char* filename, std::vector<RgnPix>& centeringPnt)
|
|
{
|
|
if (centeringPnt.size() < 6)
|
|
return;
|
|
|
|
int vldSize = (int)centeringPnt.size() - 6;
|
|
if ( (vldSize % 9) != 0)
|
|
return;
|
|
int grp = vldSize / 9;
|
|
|
|
std::ofstream TXTFile(filename);
|
|
char TXTData[250];
|
|
snprintf(TXTData, sizeof(TXTData), "%3x", (0x800 | 0x10));//MEANSTG_WIN_SIZE);
|
|
TXTFile << TXTData << std::endl;
|
|
|
|
int headOffset = 6;
|
|
for (int i = 0; i < grp; i++)
|
|
{
|
|
RgnPix data = centeringPnt[i * 9 + headOffset];
|
|
snprintf(TXTData, sizeof(TXTData), "%04x %04x %04x %01x %02x", data.LazerWinX, data.LazerWinY, data.LazerWinRid, data.LazerWinFlag, data.RltvRdx);
|
|
TXTFile << TXTData << std::endl;
|
|
for (int j = 1; j <= 8; j++)
|
|
{
|
|
data = centeringPnt[i * 9 + j + headOffset];
|
|
snprintf(TXTData, sizeof(TXTData), "%02x", data.LazerWinX);
|
|
TXTFile << TXTData << std::endl;
|
|
snprintf(TXTData, sizeof(TXTData), "%02x", data.LazerWinY);
|
|
TXTFile << TXTData << std::endl;
|
|
}
|
|
}
|
|
TXTFile.close();
|
|
|
|
}
|
|
|
|
void saveSubpixData(char* filename, std::vector<RgnSubPix>& subpixPnt)
|
|
{
|
|
if (subpixPnt.size() < 6)
|
|
return;
|
|
|
|
std::ofstream TXTFile(filename);
|
|
char TXTData[250];
|
|
int headOffset = 6;
|
|
for (int i = 6, i_max = (int)subpixPnt.size(); i< i_max; i++)
|
|
{
|
|
RgnSubPix data = subpixPnt[i];
|
|
snprintf(TXTData, sizeof(TXTData), "%.5f %04d rid=%04d flag=%01x value=%03d", data.x, data.y, data.rid, data.flag, data.value);
|
|
TXTFile << TXTData << std::endl;
|
|
}
|
|
TXTFile.close();
|
|
}
|
|
|
|
void saveCalibSubpixData(char* filename, std::vector<RgnSubPixCalib>& subpixPnt)
|
|
{
|
|
if (subpixPnt.size() < 6)
|
|
return;
|
|
|
|
std::ofstream TXTFile(filename);
|
|
char TXTData[250];
|
|
int headOffset = 6;
|
|
for (int i = 6, i_max = (int)subpixPnt.size(); i < i_max; i++)
|
|
{
|
|
RgnSubPixCalib data = subpixPnt[i];
|
|
snprintf(TXTData, sizeof(TXTData), "%.5f %.5f rid=%04d flag=%01x value=%03d", data.x, data.y, data.rid, data.flag, data.value);
|
|
TXTFile << TXTData << std::endl;
|
|
}
|
|
TXTFile.close();
|
|
}
|
|
|
|
|
|
void readCalibK(char* paraFilename, LineCalibK* lineCalibK)
|
|
{
|
|
std::ifstream inputFile(paraFilename);
|
|
std::string linedata;
|
|
|
|
if (inputFile.is_open() == false)
|
|
return;
|
|
int i = 0;
|
|
while (std::getline(inputFile, linedata))
|
|
{
|
|
LineCalibK a_para;
|
|
sscanf_s(linedata.c_str(), "%lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf", &a_para.calibK[0], &a_para.calibK[1], &a_para.calibK[2], &a_para.calibK[3], &a_para.calibK[4], &a_para.calibK[5], &a_para.calibK[6], &a_para.calibK[7]);
|
|
lineCalibK[i] = a_para;
|
|
i++;
|
|
}
|
|
inputFile.close();
|
|
return;
|
|
}
|
|
|
|
CamPara readLaserPlanePara(char* filename)
|
|
{
|
|
|
|
CamPara para;
|
|
memset(¶, 0, sizeof(CamPara));
|
|
std::ifstream inputFile(filename);
|
|
std::string linedata;
|
|
|
|
if (inputFile.is_open() == false)
|
|
return para;
|
|
|
|
//读取: fx, 0, cx
|
|
if (std::getline(inputFile, linedata))
|
|
{
|
|
double tmp;
|
|
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", ¶.F_inv, &tmp, ¶.u0);
|
|
para.F_inv = 1.0 / para.F_inv;
|
|
}
|
|
else
|
|
return para;
|
|
|
|
//读取: 0, fy, cy
|
|
if (std::getline(inputFile, linedata))
|
|
{
|
|
double tmp_1, tmp_2;
|
|
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &tmp_1, &tmp_2, ¶.v0);
|
|
}
|
|
else
|
|
return para;
|
|
|
|
//跳过现行
|
|
if (!std::getline(inputFile, linedata))
|
|
return para;
|
|
if (!std::getline(inputFile, linedata))
|
|
return para;
|
|
|
|
//读取: a, b, c
|
|
if (std::getline(inputFile, linedata))
|
|
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", ¶.plane_a, ¶.plane_b, ¶.plane_c);
|
|
|
|
return para;
|
|
}
|
|
|
|
void saveLineData(char* fileName, std::vector< Pnt3D>& resul3DPnts)
|
|
{
|
|
std::ofstream sw(fileName);
|
|
sw << "LineNum:1" << std::endl;
|
|
// sw << "DataType: 0" << std::endl;
|
|
// sw << "ScanSpeed: 0" << std::endl;
|
|
// sw << "PointAdjust:1" << std::endl;
|
|
// sw << "MaxTimeStamp:0_0" << std::endl;
|
|
|
|
int ptCounter = (int)resul3DPnts.size() - 4;
|
|
sw << "Line_0_0_" << ptCounter << std::endl;
|
|
for (int i = 4; i < (int)resul3DPnts.size(); i++)
|
|
{
|
|
Pnt3D a_pt = resul3DPnts[i];
|
|
float x = (float)a_pt.x;
|
|
float y = (float)a_pt.y;
|
|
float z = (float)a_pt.z;
|
|
sw << "{" << x << "," << y << "," << z << "}-";
|
|
sw << "{0,0}-{0,0}" << std::endl;
|
|
}
|
|
sw.close();
|
|
}
|
|
|
|
#define CALIB_TEST_GROUP 3
|
|
int main()
|
|
{
|
|
std::cout << "Hello World!\n";
|
|
const char* calibDataPath[CALIB_TEST_GROUP] = {
|
|
"F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\chessboard\\", //1
|
|
"F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\circlePoint\\", //2
|
|
"F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\charuCo\\", //3
|
|
};
|
|
const SWdNLRange fileIdx[CALIB_TEST_GROUP] = {
|
|
{1,33},{1,33},{1,10}
|
|
};
|
|
|
|
//算法参数
|
|
ProcObj frontendPara;
|
|
frontendPara.StartIdx = 0;
|
|
frontendPara.EndIdx = 0;
|
|
frontendPara.MinStgTh = 5;
|
|
frontendPara.LazerWMin = 2;
|
|
frontendPara.LazerWMax = 10;
|
|
frontendPara.RflctPixTh = 128;
|
|
frontendPara.RflctOutEna = 0;//??
|
|
frontendPara.OverlapPixTh = 32;
|
|
frontendPara.EnhanStep = 2;
|
|
frontendPara.PickEna = 1;
|
|
frontendPara.RgnFltrTh = 0;
|
|
frontendPara.RmvWkEndEna = 1;
|
|
frontendPara.RmvWkEndTh = 2;
|
|
frontendPara.RmvWkEndMultCoe = 16;
|
|
frontendPara.RmvWkEndMinLen = 3;
|
|
frontendPara.PickBFEna = 0;
|
|
frontendPara.PickBkGrnd = 0;
|
|
frontendPara.PickRLenTh = 4;
|
|
frontendPara.EnergyPickEna = 1;
|
|
frontendPara.PickEnergyType = 0;
|
|
frontendPara.RgnEnergyPLen = 32;
|
|
frontendPara.RgnMeanETh = 0;
|
|
|
|
LineCalibK* lineCalibKx = (LineCalibK*)malloc(sizeof(LineCalibK) * MAX_HEIGHT);
|
|
LineCalibK* lineCalibKy = (LineCalibK*)malloc(sizeof(LineCalibK) * MAX_HEIGHT);
|
|
|
|
FramePara inFramePara;
|
|
CamPara calibPara;
|
|
|
|
for (int grp = 0; grp < CALIB_TEST_GROUP; grp++)
|
|
{
|
|
memset(lineCalibKx, 0, sizeof(LineCalibK) * MAX_HEIGHT);
|
|
memset(lineCalibKy, 0, sizeof(LineCalibK) * MAX_HEIGHT);
|
|
//读取校正系数
|
|
char paraFilename[256];
|
|
sprintf_s(paraFilename, "%scalib_param_x.txt", calibDataPath[grp]);
|
|
readCalibK(paraFilename, lineCalibKx);
|
|
sprintf_s(paraFilename, "%scalib_param_y.txt", calibDataPath[grp]);
|
|
readCalibK(paraFilename, lineCalibKy);
|
|
//读取光刀面
|
|
sprintf_s(paraFilename, "%scalib_param_K_D.txt", calibDataPath[grp]);
|
|
calibPara = readLaserPlanePara(paraFilename);
|
|
|
|
int startIndex = fileIdx[grp].nMin;
|
|
int endIndex = fileIdx[grp].nMax;
|
|
int index;
|
|
for (index = startIndex; index <= endIndex; index++)
|
|
{
|
|
char filename[256];
|
|
sprintf_s(filename, "%slaser_rotate_mask_%03d.png", calibDataPath[grp], index);
|
|
cv::Mat srcImg = cv::imread(filename);
|
|
if (srcImg.empty())
|
|
break;
|
|
|
|
inFramePara.FrmNo = index;
|
|
inFramePara.timeStamp = index;
|
|
inFramePara.encInfo = 0;
|
|
inFramePara.frameROI_w = srcImg.cols;
|
|
inFramePara.frameROI_h = srcImg.rows;
|
|
inFramePara.frameROI_x = 0;
|
|
inFramePara.frameROI_y = 0;
|
|
|
|
cv::Mat gray;
|
|
if (srcImg.channels() == 3)
|
|
cv::cvtColor(srcImg, gray, cv::COLOR_BGR2GRAY);
|
|
else
|
|
gray = srcImg.clone();
|
|
|
|
unsigned char* dataPtr = gray.data;
|
|
|
|
std::vector< Pnt3D> resul3DPnts;
|
|
#if _ALGO_MODULE_DEBUG
|
|
std::vector<Luma_rgnData> frontendData;
|
|
std::vector<RgnPix> centeringPnt;
|
|
std::vector<RgnSubPix> subpixPnt;
|
|
std::vector<RgnSubPixCalib> calibSubpixPnt;
|
|
#endif
|
|
camAlgoSW(
|
|
gray.rows,
|
|
gray.cols,
|
|
dataPtr,
|
|
frontendPara,
|
|
inFramePara,
|
|
calibPara,
|
|
lineCalibKx,
|
|
lineCalibKy,
|
|
#if _ALGO_MODULE_DEBUG
|
|
frontendData,
|
|
centeringPnt,
|
|
subpixPnt,
|
|
calibSubpixPnt,
|
|
#endif
|
|
resul3DPnts);
|
|
#if _ALGO_MODULE_DEBUG
|
|
//保存frontendData
|
|
sprintf_s(filename, "%sresult\\%d_verilog.txt", calibDataPath[grp], index);
|
|
saveFrontEndData(filename, frontendData);
|
|
//保存centeringData
|
|
sprintf_s(filename, "%sresult\\%d_centeringData.txt", calibDataPath[grp], index);
|
|
saveCenteringData(filename, centeringPnt);
|
|
//保存subpixData
|
|
sprintf_s(filename, "%sresult\\%d_subpixData.txt", calibDataPath[grp], index);
|
|
saveSubpixData(filename, subpixPnt);
|
|
//显示亚像素点
|
|
cv::Mat enlargeImg;
|
|
if (srcImg.channels() == 1)
|
|
srcImg.convertTo(enlargeImg, cv::COLOR_GRAY2BGR);
|
|
else
|
|
enlargeImg = srcImg.clone();
|
|
cv::Size objSize = srcImg.size();
|
|
objSize.width = objSize.width * 5;
|
|
cv::resize(enlargeImg, enlargeImg, objSize, 0, 0, cv::INTER_NEAREST);
|
|
for (int i = 6, i_max = (int)subpixPnt.size(); i < i_max; i++)
|
|
{
|
|
RgnSubPix a_subPix = subpixPnt[i];
|
|
int row = (int)(a_subPix.y + 0.5);
|
|
int col = (int)(a_subPix.x * 5 + 0.5);
|
|
enlargeImg.at<cv::Vec3b>(row, col)[0] = 0;
|
|
enlargeImg.at<cv::Vec3b>(row, col)[1] = 0;
|
|
enlargeImg.at<cv::Vec3b>(row, col)[2] = 255;
|
|
}
|
|
sprintf_s(filename, "%sresult\\%d_subpix.png", calibDataPath[grp], index);
|
|
cv::imwrite(filename, enlargeImg);
|
|
|
|
//保存校正后的subpixData
|
|
sprintf_s(filename, "%sresult\\%d_calibSubpixData.txt", calibDataPath[grp], index);
|
|
saveCalibSubpixData(filename, calibSubpixPnt);
|
|
|
|
#endif
|
|
sprintf_s(filename, "%sresult\\%d_3D_data.txt", calibDataPath[grp], index);
|
|
saveLineData(filename, resul3DPnts);
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|