diff --git a/CamAlgo.sln b/CamAlgo.sln index 82eec9e..3bcdccc 100644 --- a/CamAlgo.sln +++ b/CamAlgo.sln @@ -12,6 +12,9 @@ EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "camAlgoSW", "camAlgoSW\camAlgoSW.vcxproj", "{C280CB49-2B9B-43B9-A99E-5C90B7CEF033}" EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "camAlgoSW_test", "camAlgoSW_test\camAlgoSW_test.vcxproj", "{AA8276D9-77FD-451F-BC95-AC8E77D9CEFC}" + ProjectSection(ProjectDependencies) = postProject + {C280CB49-2B9B-43B9-A99E-5C90B7CEF033} = {C280CB49-2B9B-43B9-A99E-5C90B7CEF033} + EndProjectSection EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "camCalib", "camCalib\camCalib.vcxproj", "{3C3A4670-25E6-4E17-9723-2897316D2437}" EndProject diff --git a/camAlgoSW/camAlgoSW.vcxproj b/camAlgoSW/camAlgoSW.vcxproj index 03e2baa..9baf6c7 100644 --- a/camAlgoSW/camAlgoSW.vcxproj +++ b/camAlgoSW/camAlgoSW.vcxproj @@ -97,10 +97,12 @@ true .\sourceCode;.\sourceCode\CG_frontEnd\inc;$(VC_IncludePath);$(WindowsSDK_IncludePath); + $(SolutionDir)build\$(Platform)\$(Configuration)\ false .\sourceCode;.\sourceCode\CG_frontEnd\inc;$(VC_IncludePath);$(WindowsSDK_IncludePath); + $(SolutionDir)build\$(Platform)\$(Configuration)\ diff --git a/camAlgoSW/sourceCode/camAlgoSW.cpp b/camAlgoSW/sourceCode/camAlgoSW.cpp index c1a5a7e..ab57a75 100644 --- a/camAlgoSW/sourceCode/camAlgoSW.cpp +++ b/camAlgoSW/sourceCode/camAlgoSW.cpp @@ -10,17 +10,6 @@ #include "compute3D.h" #include "camAlgoSW_Export.h" -typedef struct -{ - uint16_t WinRdx;//窗口在图像内的起始坐标 - uint16_t y; //y坐标 - uint16_t Rid; //region的ID - uint8_t Flag; //bit0是overlap标志。bit1是反光信号标志 - uint8_t PeakRltvRdx; //peak在窗口内的的相对坐标，低4位是起始坐标，高4位是结束坐标 - uint8_t data[RGN_DATA_WIN_SIZE]; - //double offset; -}Luma_rgnData; - #define SV_WIN_SIZE 16 #define ALIGN_RDX ((LAZER_WIN_SIZE-SV_WIN_SIZE)/2) std::vector SvPickRgnRslt( @@ -280,8 +269,15 @@ void camAlgoSW( CamPara& calibPara, LineCalibK* lineCalibKx, LineCalibK* lineCalibKy, +#if _ALGO_MODULE_DEBUG + std::vector& frontendData_debug, + std::vector& centeringPnt_debug, + std::vector& subpixPnt_debug, + std::vector& calibSubpixPnt_debug, +#endif std::vector& resul3DPnts) { + PickLaserResult pFrontendResult; PickLazerWin( 1, @@ -312,15 +308,17 @@ void camAlgoSW( frontendPara.RgnMeanETh, &pFrontendResult ); - //将前端数据转成输入数据格式 - std::vector frontendData = SvPickRgnRslt( inData, inFrmH, inFrmW, pFrontendResult ); +#if _ALGO_MODULE_DEBUG + frontendData_debug.insert(frontendData_debug.end(), frontendData.begin(), frontendData.end()); +#endif + std::vector InRgnPnt; genTestSream( frontendData, @@ -332,6 +330,9 @@ void camAlgoSW( InRgnPnt, OutCenteringPnt ); +#if _ALGO_MODULE_DEBUG + centeringPnt_debug.insert(centeringPnt_debug.end(), OutCenteringPnt.begin(), OutCenteringPnt.end()); +#endif //Convolve std::vector OutConvolvePnt; @@ -344,6 +345,9 @@ void camAlgoSW( subpix( OutConvolvePnt, OutSubpixPnt); +#if _ALGO_MODULE_DEBUG + subpixPnt_debug.insert(subpixPnt_debug.end(), OutSubpixPnt.begin(), OutSubpixPnt.end()); +#endif //将subpix转成RgnSubPixCalib格式。在硬件实现时这一步骤由硬件完成 //数据格式：每行由行同步和后续的8拍构成。行同步传送x和y数据，后8拍依次传送KX0~KX7, 和KY0 ~ KY7 @@ -357,6 +361,9 @@ void camAlgoSW( inCalibData, outSubpixCalib ); +#if _ALGO_MODULE_DEBUG + calibSubpixPnt_debug.insert(calibSubpixPnt_debug.end(), outSubpixCalib.begin(), outSubpixCalib.end()); +#endif // Compute3D compute3D( diff --git a/camAlgoSW/sourceCode/camAlgoSW_Export.h b/camAlgoSW/sourceCode/camAlgoSW_Export.h index a75065a..b56fb21 100644 --- a/camAlgoSW/sourceCode/camAlgoSW_Export.h +++ b/camAlgoSW/sourceCode/camAlgoSW_Export.h @@ -9,6 +9,18 @@ # define SG_APISHARED_EXPORT __declspec(dllimport) #endif +typedef struct +{ + uint16_t WinRdx;//窗口在图像内的起始坐标 + uint16_t y; //y坐标 + uint16_t Rid; //region的ID + uint8_t Flag; //bit0是overlap标志。bit1是反光信号标志 + uint8_t PeakRltvRdx; //peak在窗口内的的相对坐标，低4位是起始坐标，高4位是结束坐标 + uint8_t data[RGN_DATA_WIN_SIZE]; + //double offset; +}Luma_rgnData; + +#define _ALGO_MODULE_DEBUG 1 SG_APISHARED_EXPORT void camAlgoSW( int inFrmH, int inFrmW, @@ -18,4 +30,10 @@ SG_APISHARED_EXPORT void camAlgoSW( CamPara& calibPara, LineCalibK* lineCalibKx, LineCalibK* lineCalibKy, +#if _ALGO_MODULE_DEBUG + std::vector& frontendData, + std::vector& centeringPnt_debug, + std::vector& subpixPnt_debug, + std::vector& calibSubpixPnt_debug, +#endif std::vector< Pnt3D>& resul3DPnts); \ No newline at end of file diff --git a/camAlgoSW/sourceCode/compute3D.cpp b/camAlgoSW/sourceCode/compute3D.cpp index d032839..55a696f 100644 --- a/camAlgoSW/sourceCode/compute3D.cpp +++ b/camAlgoSW/sourceCode/compute3D.cpp @@ -62,10 +62,14 @@ void compute3D( double u = (double)InData.x; double v = (double)InData.y; - double z_inv = u * plane_a + v * plane_b + plane_c; - double z = 1/ z_inv; - double x = (u - u0) * z * F_inv; - double y = (v - v0) * z * F_inv; + double tmp_x = (u - u0) * F_inv; + double tmp_y = (v - v0) * F_inv; + double z_inv = plane_a * tmp_x + plane_b * tmp_y - 1; + double z = 1 / z_inv; + z = -z * plane_c; + + double x = tmp_x * z ; + double y = tmp_y * z ; outData.x = (float)x; outData.y = (float)y; outData.z = (float)z; diff --git a/camAlgoSW/sourceCode/peakCentering.cpp b/camAlgoSW/sourceCode/peakCentering.cpp index b286ac5..7f16dcd 100644 --- a/camAlgoSW/sourceCode/peakCentering.cpp +++ b/camAlgoSW/sourceCode/peakCentering.cpp @@ -49,6 +49,7 @@ void peakCentering( RgnPix zeroData = {0,0,0,0,0,0,0}; //ping-pong buffer RgnPix HSyncData_d1 = {0,0,0,0,0,0,0}; + ushort LazerWinX_d1 = 0; uchar lineBuff_0[RGN_DATA_WIN_SIZE * 2]; //#pragma HLS RESOURCE variable=lineBuff_0 core=RAM_2P uchar lineBuff_1[RGN_DATA_WIN_SIZE * 2]; @@ -82,9 +83,18 @@ void peakCentering( if(0 == i) //HSync { + evenFlag = n % 2; + if (0 == linePk.len) + pkCenter = RGN_DATA_WIN_SIZE / 2; + else + pkCenter = linePk.start + (linePk.len >> 1); + HSyncData_d1.LazerWinX = LazerWinX_d1 + pkCenter; + if (HSyncData_d1.LazerWinY == 578) + int kkk = 1; if ((n > 0) &&( linePk.len>0)) OutCenteringPnt.push_back(HSyncData_d1); HSyncData_d1 = InData; + LazerWinX_d1 = InData.LazerWinX - RGN_DATA_WIN_SIZE; pkRng_S = InData.RltvRdx & 0x0f; pkRng_S += RGN_DATA_WIN_SIZE / 2; @@ -92,11 +102,6 @@ void peakCentering( pkRng_E = InData.RltvRdx & 0x0f; pkRng_E += RGN_DATA_WIN_SIZE / 2; - evenFlag = n%2; - if(0 == linePk.len) - pkCenter = RGN_DATA_WIN_SIZE/2; - else - pkCenter = linePk.start + (linePk.len >> 1); linePk.start = 0; linePk.len = 0; linePk.value = 0; diff --git a/camAlgoSW/sourceCode/subPix.cpp b/camAlgoSW/sourceCode/subPix.cpp index 87418b3..428d0dc 100644 --- a/camAlgoSW/sourceCode/subPix.cpp +++ b/camAlgoSW/sourceCode/subPix.cpp @@ -84,6 +84,8 @@ void subpix( InData = InConvolvePnt[readPtr++]; + if (InData.LazerWinY == 579) + int kkk = 1; float fx; if(InData.nD2 == 0) fx = 0; @@ -91,7 +93,7 @@ void subpix( fx = (float)InData.nD1/(float)InData.nD2; RgnSubPix a_pnt; - if(n == 1) + if(n == 0) a_pnt.Sync = 0b01; else a_pnt.Sync = 0b00; @@ -99,7 +101,7 @@ void subpix( a_pnt.y = InData.LazerWinY; a_pnt.rid = InData.LazerWinRid; a_pnt.value = InData.value; - a_pnt.x = (float)InData.LazerWinX + 8.0f + fx; + a_pnt.x = (float)InData.LazerWinX + 8.0f + fx + 0.5; //+0.5: adjust pnt to pixel center a_pnt.rsv = 0; OutSubpixPnt.push_back(a_pnt); } diff --git a/camAlgoSW_test/camAlgoSW_test.cpp b/camAlgoSW_test/camAlgoSW_test.cpp index 00fcc8e..377d1d0 100644 --- a/camAlgoSW_test/camAlgoSW_test.cpp +++ b/camAlgoSW_test/camAlgoSW_test.cpp @@ -2,10 +2,387 @@ // #include +#include +#include "..\camAlgoSW\sourceCode\camAlgoSW_Export.h" +#include +#include +#include "..\camAlgoSW\sourceCode\algoGlobals.h" -int main() +typedef struct { - std::cout << "Hello World!\n"; + int nMin; //< 鏈灏忓 + int nMax; //< 鏈澶у +} SWdNLRange; + +void saveFrontEndData(char* fileName, std::vector& 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& 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& 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& 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& 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 frontendData; + std::vector centeringPnt; + std::vector subpixPnt; + std::vector 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(row, col)[0] = 0; + enlargeImg.at(row, col)[1] = 0; + enlargeImg.at(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); + } + } + } diff --git a/camAlgoSW_test/camAlgoSW_test.vcxproj b/camAlgoSW_test/camAlgoSW_test.vcxproj index d9274f9..9c59262 100644 --- a/camAlgoSW_test/camAlgoSW_test.vcxproj +++ b/camAlgoSW_test/camAlgoSW_test.vcxproj @@ -78,11 +78,13 @@ true - F:\ShangGu\ProductDev\AlgoDev\thirdParty\opencv\build\include;$(IncludePath) + F:\ShangGu\ProductDev\AlgoDev\thirdParty\opencv\build\include;$(IncludePath);F:\ShangGu\ProductDev\涓夎鍏夌浉鏈篭鐩告満寮鍙慭camAlgo_git\camAlgoSW\sourceCode + $(SolutionDir)build\$(Platform)\$(Configuration)\ false - F:\ShangGu\ProductDev\AlgoDev\thirdParty\opencv\build\include;$(VC_IncludePath);$(WindowsSDK_IncludePath); + F:\ShangGu\ProductDev\AlgoDev\thirdParty\opencv\build\include;$(VC_IncludePath);$(WindowsSDK_IncludePath);;F:\ShangGu\ProductDev\涓夎鍏夌浉鏈篭鐩告満寮鍙慭camAlgo_git\camAlgoSW\sourceCode + $(SolutionDir)build\$(Platform)\$(Configuration)\ @@ -122,6 +124,8 @@ Console true + F:\ShangGu\ProductDev\AlgoDev\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;.\sourceCode\CG_frontEnd\lib\debug;%(AdditionalLibraryDirectories) + opencv_world480d.lib;camAlgoSW.lib;ImgAccd.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) @@ -132,12 +136,15 @@ true NDEBUG;_CONSOLE;%(PreprocessorDefinitions) true + Disabled Console true true true + F:\ShangGu\ProductDev\AlgoDev\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;F:\ShangGu\ProductDev\涓夎鍏夌浉鏈篭鐩告満寮鍙慭camAlgo_git\camAlgoSW\sourceCode\CG_frontEnd\lib\debug;%(AdditionalLibraryDirectories) + opencv_world480.lib;camAlgoSW.lib;ImgAccd.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) diff --git a/camCalib/camCalib.cpp b/camCalib/camCalib.cpp index 675caa0..510e1a3 100644 --- a/camCalib/camCalib.cpp +++ b/camCalib/camCalib.cpp @@ -29,7 +29,7 @@ void sg_outputCalibK(const char* fileName, cv::Mat& fitMap) return; } -void sg_outputCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D) +void sg_outputCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D, cv::Vec4f& pe) { std::ofstream sw(fileName); @@ -50,6 +50,20 @@ void sg_outputCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D) temp[_c] = D.ptr(0)[_c]; sprintf_s(dataStr, 250, "%g, %g, %g, %g, %g", temp[0], temp[1], temp[2], temp[3], temp[4]); sw << dataStr << std::endl; + + //ax+by+cz+d = 0 + float a = (float)pe[0]; + float b = (float)pe[1]; + float c = (float)pe[2]; + float d = (float)pe[3]; + //灏哻鍙樻垚-1锛岃浆鎴恴=ax+by+c鐨勫舰寮忥紝浣跨敤3涓弬鏁 + a = -a / c; + b = -b / c; + d = -d / c; + c = -1; + sprintf_s(dataStr, 250, "%g, %g, %g", a, b, d); + sw << dataStr << std::endl; + sw.close(); return; } @@ -87,6 +101,24 @@ void sg_readCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D) return; } +void saveSubpixData(char* filename, std::vector& 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++) + { + cv::Point2f a_subPix = subpixPnt[i]; + snprintf(TXTData, sizeof(TXTData), "%.5f %.5f", a_subPix.x, a_subPix.y); + TXTFile << TXTData << std::endl; + } + TXTFile.close(); +} + + typedef struct { int nMin; //< 鏈灏忓 @@ -130,13 +162,13 @@ int main() std::cout << "Hello World!\n"; const char* calibDataPath[CALIB_TEST_GROUP] = { - "F:\\ShangGu\\ProductDev\\涓夎鍏夌浉鏈篭\鐩告満寮鍙慭\CamAlgo\\camCalib\\camCalibData\\鎾曡鍘熺悊鐩告満鏍囧畾鍥惧儚\\", //0 - "F:\\ShangGu\\ProductDev\\涓夎鍏夌浉鏈篭\鐩告満寮鍙慭\CamAlgo\\camCalib\\camCalibData\\chessboard\\", //1 - "F:\\ShangGu\\ProductDev\\涓夎鍏夌浉鏈篭\鐩告満寮鍙慭\CamAlgo\\camCalib\\camCalibData\\circlePoint\\", //2 - "F:\\ShangGu\\ProductDev\\涓夎鍏夌浉鏈篭\鐩告満寮鍙慭\CamAlgo\\camCalib\\camCalibData\\charuCo\\", //3 + "F:\\ShangGu\\ProductDev\\涓夎鍏夌浉鏈篭\鐩告満寮鍙慭\CamAlgo_git\\camCalib\\camCalibData\\鎾曡鍘熺悊鐩告満鏍囧畾鍥惧儚\\", //0 + "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] = { - {3,39},{1,200},{1,166},{122,141} + {3,39},{1,33},{1,33},{1,10} }; const int boardType[CALIB_TEST_GROUP] = { @@ -148,7 +180,7 @@ int main() for(int grp = 0; grp < CALIB_TEST_GROUP; grp ++) { - grp = 2; + grp = 1; int calibType = boardType[grp]; cv::Size cbPattern; float cbSquareSize; @@ -283,9 +315,7 @@ int main() std::cout << "Y Mean difference: " << meanVal_y[0] << std::endl; //鐢熸垚鐭鍥惧儚 - index = 3; - for (;; index++) { - + for (index = startIndex; index <= endIndex; index++) { char filename[256]; sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index); cv::Mat srcImg = cv::imread(filename); @@ -306,10 +336,6 @@ int main() cv::imwrite(filename, calibImg); } - //output K and D - char calibKDName[256]; - sprintf_s(calibKDName, "%scalib_param_K_D.txt", calibDataPath[grp]); - sg_outputCalibKD(calibKDName, K, D); #else char calibKDName[256]; sprintf_s(calibKDName, "%scalib_param_K_D.txt", cbImagePath); @@ -354,136 +380,156 @@ int main() #endif - { - std::vector all_pts3d; + std::vector all_pts3d; + for (index = startIndex; index <= endIndex; index++) { - index = 3; - for (;; index++) { + char filename[256]; + sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index); + cv::Mat srcImg = cv::imread(filename); + if (srcImg.empty()) + break; - char filename[256]; - sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index); - cv::Mat srcImg = cv::imread(filename); - if (srcImg.empty()) - break; + cv::Mat img; + cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE); + std::vector corners; + detectCorners(img, cbPattern, corners); + if (corners.empty()) + continue; - cv::Mat img; - cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE); - std::vector corners; - detectCorners(img, cbPattern, corners); - if (corners.empty()) - continue; - - // 鍒涘缓妫嬬洏鏍煎尯鍩熺殑鎺╃爜 - cv::Mat chessMask = cv::Mat::zeros(img.size(), CV_8UC1); - // 浣跨敤澶氳竟褰㈣繎浼兼潵濉厖瑙掔偣涔嬮棿鐨勫尯鍩 - // 妫嬬洏鏍煎尯鍩熼渶瑕佹瘮瑙掔偣鍖哄煙澶т竴鍦 - std::vector contour_line[4]; - for (int i = 0; i < cbPattern.width; i++) - { - cv::Point2f pt_c = corners[i]; - cv::Point2f pt_2 = corners[cbPattern.width + i]; - cv::Point2f pt_1; - pt_1.x = pt_c.x * 2 - pt_2.x; - pt_1.y = pt_c.y * 2 - pt_2.y; - contour_line[0].push_back(pt_1); - } - for (int i = 0; i < cbPattern.height; i++) - { - cv::Point2f pt_c = corners[i * cbPattern.width + cbPattern.width - 1]; - cv::Point2f pt_2 = corners[i * cbPattern.width + cbPattern.width - 2]; - cv::Point2f pt_1; - pt_1.x = pt_c.x * 2 - pt_2.x; - pt_1.y = pt_c.y * 2 - pt_2.y; - contour_line[1].push_back(pt_1); - } - for (int i = cbPattern.width - 1; i >= 0; i--) - { - cv::Point2f pt_c = corners[(cbPattern.height - 1) * cbPattern.width + i]; - cv::Point2f pt_2 = corners[(cbPattern.height - 2) * cbPattern.width + i]; - cv::Point2f pt_1; - pt_1.x = pt_c.x * 2 - pt_2.x; - pt_1.y = pt_c.y * 2 - pt_2.y; - contour_line[2].push_back(pt_1); - } - for (int i = cbPattern.height-1; i >= 0; i--) - { - cv::Point2f pt_c = corners[i * cbPattern.width]; - cv::Point2f pt_2 = corners[i * cbPattern.width + 1]; - cv::Point2f pt_1; - pt_1.x = pt_c.x * 2 - pt_2.x; - pt_1.y = pt_c.y * 2 - pt_2.y; - contour_line[3].push_back(pt_1); - } - std::vector contours; - //鐢熸垚杞粨鐐 - for (int n = 0; n < 4; n++) - { - int num = contour_line[n].size(); - for (int i = 0; i < num; i++) - contours.push_back(contour_line[n][i]); - cv::Point2f pt_c = contour_line[n][num - 1]; - cv::Point2f pt_2 = contour_line[n][num - 2]; - cv::Point2f pt_1; - pt_1.x = pt_c.x * 2 - pt_2.x; - pt_1.y = pt_c.y * 2 - pt_2.y; - contours.push_back(pt_1); - } - // 浣跨敤 fillPoly 濉厖澶氳竟褰 - cv::Scalar color(255); // 绾㈣壊 - cv::fillPoly(chessMask, contours, color); -#if 1 - sprintf_s(filename, "%schessMask_%03d.png", calibDataPath[grp], index); - cv::imwrite(filename, chessMask); -#endif - - cv::Vec4f pe; - fitChessboardPlane(corners, K, D, cbPattern, cbSquareSize, pe); - - sprintf_s(filename, "%slaser_%03d.bmp", calibDataPath[grp], index); - cv::Mat srcLaserImg = cv::imread(filename); - if (srcLaserImg.empty()) - break; - - cv::Mat laserImg_unMask; - cv::rotate(srcLaserImg, laserImg_unMask, cv::ROTATE_90_COUNTERCLOCKWISE); - //涓嶮ask鐩镐笌锛屼繚璇佸緟澶勭悊鐨勬縺鍏夌嚎鍦ㄦ爣瀹氭澘涓 - cv::Mat laserImg; - cv::bitwise_and(laserImg_unMask, laserImg_unMask, laserImg, chessMask); -#if 1 - sprintf_s(filename, "%slaser_rotate_mask_%03d.png", calibDataPath[grp], index); - cv::imwrite(filename, laserImg); -#endif - std::vector pts2d = detectLaserLine(laserImg); - //鏄剧ず浜氬儚绱犵偣 - cv::Mat enlargeImg; - if (laserImg.channels() == 1) - laserImg.convertTo(enlargeImg, cv::COLOR_GRAY2BGR); - else - enlargeImg = laserImg.clone(); - cv::Size objSize = laserImg.size(); - objSize.width = objSize.width * 5; - cv::resize(enlargeImg, enlargeImg, objSize, 0, 0, cv::INTER_NEAREST); - - for (int i = 0, i_max = pts2d.size(); i < i_max; i++) - { - cv::Point2f a_subPix = pts2d[i]; - a_subPix.x += 0.5; - int row = (int)(a_subPix.y + 0.5); - int col = (int)(a_subPix.x * 5 + 0.5); - enlargeImg.at(row, col)[0] = 0; - enlargeImg.at(row, col)[1] = 0; - enlargeImg.at(row, col)[2] = 255; - } - sprintf_s(filename, "%slaser_rotate_enlarge_%03d_subpix.png", calibDataPath[grp], index); - cv::imwrite(filename, enlargeImg); - std::vector pts3d = project2DTo3D(pts2d, pe, K, D); - - all_pts3d.insert(all_pts3d.end(), pts3d.begin(), pts3d.end()); + // 鍒涘缓妫嬬洏鏍煎尯鍩熺殑鎺╃爜 + cv::Mat chessMask = cv::Mat::zeros(img.size(), CV_8UC1); + // 浣跨敤澶氳竟褰㈣繎浼兼潵濉厖瑙掔偣涔嬮棿鐨勫尯鍩 + // 妫嬬洏鏍煎尯鍩熼渶瑕佹瘮瑙掔偣鍖哄煙澶т竴鍦 + std::vector contour_line[4]; + for (int i = 0; i < cbPattern.width; i++) + { + cv::Point2f pt_c = corners[i]; + cv::Point2f pt_2 = corners[cbPattern.width + i]; + cv::Point2f pt_1; + pt_1.x = pt_c.x * 2 - pt_2.x; + pt_1.y = pt_c.y * 2 - pt_2.y; + contour_line[0].push_back(pt_1); } + for (int i = 0; i < cbPattern.height; i++) + { + cv::Point2f pt_c = corners[i * cbPattern.width + cbPattern.width - 1]; + cv::Point2f pt_2 = corners[i * cbPattern.width + cbPattern.width - 2]; + cv::Point2f pt_1; + pt_1.x = pt_c.x * 2 - pt_2.x; + pt_1.y = pt_c.y * 2 - pt_2.y; + contour_line[1].push_back(pt_1); + } + for (int i = cbPattern.width - 1; i >= 0; i--) + { + cv::Point2f pt_c = corners[(cbPattern.height - 1) * cbPattern.width + i]; + cv::Point2f pt_2 = corners[(cbPattern.height - 2) * cbPattern.width + i]; + cv::Point2f pt_1; + pt_1.x = pt_c.x * 2 - pt_2.x; + pt_1.y = pt_c.y * 2 - pt_2.y; + contour_line[2].push_back(pt_1); + } + for (int i = cbPattern.height-1; i >= 0; i--) + { + cv::Point2f pt_c = corners[i * cbPattern.width]; + cv::Point2f pt_2 = corners[i * cbPattern.width + 1]; + cv::Point2f pt_1; + pt_1.x = pt_c.x * 2 - pt_2.x; + pt_1.y = pt_c.y * 2 - pt_2.y; + contour_line[3].push_back(pt_1); + } + std::vector contours; + //鐢熸垚杞粨鐐 + for (int n = 0; n < 4; n++) + { + int num = contour_line[n].size(); + for (int i = 0; i < num; i++) + contours.push_back(contour_line[n][i]); + cv::Point2f pt_c = contour_line[n][num - 1]; + cv::Point2f pt_2 = contour_line[n][num - 2]; + cv::Point2f pt_1; + pt_1.x = pt_c.x * 2 - pt_2.x; + pt_1.y = pt_c.y * 2 - pt_2.y; + contours.push_back(pt_1); + } + // 浣跨敤 fillPoly 濉厖澶氳竟褰 + cv::Scalar color(255); // 绾㈣壊 + cv::fillPoly(chessMask, contours, color); +#if 1 + sprintf_s(filename, "%schessMask_%03d.png", calibDataPath[grp], index); + cv::imwrite(filename, chessMask); +#endif - cv::Vec4f pe = fitPlaneToPoints(all_pts3d); - std::cout << "pe: " << pe << std::endl; + cv::Vec4f pe; + fitChessboardPlane(corners, K, D, cbPattern, cbSquareSize, pe); + + sprintf_s(filename, "%slaser_%03d.bmp", calibDataPath[grp], index); + cv::Mat srcLaserImg = cv::imread(filename); + if (srcLaserImg.empty()) + break; + + cv::Mat laserImg_unMask; + cv::rotate(srcLaserImg, laserImg_unMask, cv::ROTATE_90_COUNTERCLOCKWISE); + //涓嶮ask鐩镐笌锛屼繚璇佸緟澶勭悊鐨勬縺鍏夌嚎鍦ㄦ爣瀹氭澘涓 + cv::Mat laserImg; + cv::bitwise_and(laserImg_unMask, laserImg_unMask, laserImg, chessMask); +#if 1 + sprintf_s(filename, "%slaser_rotate_mask_%03d.png", calibDataPath[grp], index); + cv::imwrite(filename, laserImg); + + cv::Mat calibImg; + remap(laserImg, + calibImg, + mapGen_x, + mapGen_y, + cv::INTER_LINEAR, + cv::BORDER_CONSTANT, + cv::Scalar(0, 0, 0)); + sprintf_s(filename, "%slaser_%03d_calib.bmp", calibDataPath[grp], index); + cv::imwrite(filename, calibImg); + +#endif + std::vector pts2d = detectLaserLine(laserImg); + //鏄剧ず浜氬儚绱犵偣 + cv::Mat enlargeImg; + if (laserImg.channels() == 1) + laserImg.convertTo(enlargeImg, cv::COLOR_GRAY2BGR); + else + enlargeImg = laserImg.clone(); + cv::Size objSize = laserImg.size(); + objSize.width = objSize.width * 5; + cv::resize(enlargeImg, enlargeImg, objSize, 0, 0, cv::INTER_NEAREST); + + if (pts2d.size() > 0) + { + sprintf_s(filename, "%slaser_rotate_enlarge_%03d_subpixData.txt", calibDataPath[grp], index); + saveSubpixData(filename, pts2d); + } + for (int i = 0, i_max = (int)pts2d.size(); i < i_max; i++) + { + cv::Point2f a_subPix = pts2d[i]; + int row = (int)(a_subPix.y + 0.5); + int col = (int)(a_subPix.x * 5 + 0.5); + enlargeImg.at(row, col)[0] = 0; + enlargeImg.at(row, col)[1] = 0; + enlargeImg.at(row, col)[2] = 255; + } + sprintf_s(filename, "%slaser_rotate_enlarge_%03d_subpix.png", calibDataPath[grp], index); + cv::imwrite(filename, enlargeImg); + std::vector pts3d = project2DTo3D(pts2d, pe, K, D); +#if 1 + //淇濆瓨3D鐐 + +#endif + all_pts3d.insert(all_pts3d.end(), pts3d.begin(), pts3d.end()); } + + cv::Vec4f pe = fitPlaneToPoints(all_pts3d); + std::cout << "pe: " << pe << std::endl; + + //output K and D + char calibKDName[256]; + sprintf_s(calibKDName, "%scalib_param_K_D.txt", calibDataPath[grp]); + sg_outputCalibKD(calibKDName, K, D, pe); } return 0; diff --git a/camCalib/lineDetection_steger.cpp b/camCalib/lineDetection_steger.cpp index 3d97022..26cae93 100644 --- a/camCalib/lineDetection_steger.cpp +++ b/camCalib/lineDetection_steger.cpp @@ -436,7 +436,7 @@ void computePointSubpix( if ((subpix.x >= 0) && (subpix.y >= 0)) { subpix.x += 0.5; //图像中的像素位置表示的是像素是左下角，而此处像素坐标是指向像素中间位置，所以有0.5像素差 - subpix.x -= 0.5; //此处图像的Y坐标系方向与通常坐标系的方向相反 + subpix.y -= 0.5; //此处图像的Y坐标系方向与通常坐标系的方向相反 posSubpix.push_back(subpix); doSubPix = -1; } diff --git a/camCalib/sourceCode/MonoLaserCalibrate.cpp b/camCalib/sourceCode/MonoLaserCalibrate.cpp index 6545e4f..3da2d32 100644 --- a/camCalib/sourceCode/MonoLaserCalibrate.cpp +++ b/camCalib/sourceCode/MonoLaserCalibrate.cpp @@ -191,7 +191,7 @@ void monocularCalibration( flags |= cv::fisheye::CALIB_FIX_SKEW; cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs);// , flags, cv::TermCriteria(3, 20, 1e-6)); #else - cv::calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs); + cv::calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, cv::CALIB_FIX_ASPECT_RATIO); #endif // 閲嶆姇褰变笁缁寸偣鍒颁簩缁村浘鍍忕偣 // 璁＄畻閲嶆姇褰辫宸