diff --git a/rodAndBarDetection_test/rodAndBarDetection_test.cpp b/rodAndBarDetection_test/rodAndBarDetection_test.cpp index 84ade3b..d79c59c 100644 --- a/rodAndBarDetection_test/rodAndBarDetection_test.cpp +++ b/rodAndBarDetection_test/rodAndBarDetection_test.cpp @@ -60,6 +60,73 @@ float _computeVecAngle(const SVzNL3DPoint& a, const SVzNL3DPoint& b) return degree; } +void _outputCalibPara(char* fileName, SSG_planeCalibPara calibPara) +{ + std::ofstream sw(fileName); + char dataStr[250]; + //调平矩阵 + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[0], calibPara.planeCalib[1], calibPara.planeCalib[2]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[3], calibPara.planeCalib[4], calibPara.planeCalib[5]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[6], calibPara.planeCalib[7], calibPara.planeCalib[8]); + sw << dataStr << std::endl; + //地面高度 + sprintf_s(dataStr, 250, "%g", calibPara.planeHeight); + sw << dataStr << std::endl; + //反向旋转矩阵 + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[0], calibPara.invRMatrix[1], calibPara.invRMatrix[2]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[3], calibPara.invRMatrix[4], calibPara.invRMatrix[5]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[6], calibPara.invRMatrix[7], calibPara.invRMatrix[8]); + sw << dataStr << std::endl; + + sw.close(); +} + +SSG_planeCalibPara _readCalibPara(char* fileName) +{ + //设置初始结果 + double initCalib[9] = { + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 }; + SSG_planeCalibPara planePara; + for (int i = 0; i < 9; i++) + planePara.planeCalib[i] = initCalib[i]; + planePara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + planePara.invRMatrix[i] = initCalib[i]; + + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return planePara; + + //调平矩阵 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[0], &planePara.planeCalib[1], &planePara.planeCalib[2]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[3], &planePara.planeCalib[4], &planePara.planeCalib[5]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[6], &planePara.planeCalib[7], &planePara.planeCalib[8]); + //地面高度 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf", &planePara.planeHeight); + //反向旋转矩阵 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[0], &planePara.invRMatrix[1], &planePara.invRMatrix[2]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[3], &planePara.invRMatrix[4], &planePara.invRMatrix[5]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[6], &planePara.invRMatrix[7], &planePara.invRMatrix[8]); + + inputFile.close(); + return planePara; +} + void wdReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector>& scanData) { std::ifstream inputFile(fileName); @@ -194,49 +261,38 @@ void _outputScanDataFile(char* fileName, std::vector& rodParas) { - //设置初始结果 - double initCalib[9] = { - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0 }; - SSG_planeCalibPara planePara; - for (int i = 0; i < 9; i++) - planePara.planeCalib[i] = initCalib[i]; - planePara.planeHeight = -1.0; - for (int i = 0; i < 9; i++) - planePara.invRMatrix[i] = initCalib[i]; - std::ifstream inputFile(fileName); std::string linedata; if (inputFile.is_open() == false) - return planePara; + return; - //调平矩阵 - std::getline(inputFile, linedata); - sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[0], &planePara.planeCalib[1], &planePara.planeCalib[2]); - std::getline(inputFile, linedata); - sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[3], &planePara.planeCalib[4], &planePara.planeCalib[5]); - std::getline(inputFile, linedata); - sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[6], &planePara.planeCalib[7], &planePara.planeCalib[8]); - //地面高度 - std::getline(inputFile, linedata); - sscanf_s(linedata.c_str(), "%lf", &planePara.planeHeight); - //反向旋转矩阵 - std::getline(inputFile, linedata); - sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[0], &planePara.invRMatrix[1], &planePara.invRMatrix[2]); - std::getline(inputFile, linedata); - sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[3], &planePara.invRMatrix[4], &planePara.invRMatrix[5]); - std::getline(inputFile, linedata); - sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[6], &planePara.invRMatrix[7], &planePara.invRMatrix[8]); - - inputFile.close(); - return planePara; + std::vector indices; + int maxIdx = 0; + std::vector< SSX_rodParam> paras; + while (getline(inputFile, linedata)) + { + int idx; + float len, dia; + sscanf_s(linedata.c_str(), "%d_L_%f_D_%f", &idx, &len, &dia); + SSX_rodParam a_para = { dia , len }; + indices.push_back(idx); + paras.push_back(a_para); + maxIdx = maxIdx < idx ? idx : maxIdx; + } + if (maxIdx == 0) + return; + rodParas.resize(maxIdx + 1); + for (int i = 0; i < (int)indices.size(); i++) + { + int idx = indices[i]; + rodParas[idx] = paras[i]; + } + return; } - void _outputChanneltInfo(char* fileName, std::vector& screwInfo) { std::ofstream sw(fileName); @@ -1133,23 +1189,230 @@ void newLocatingPlateTest(void) } } -#define ROD_POSITION_TEST_GROUP 3 +; +void _removeNullData(std::vector>& scanLines, double _F, std::vector>& scanData) +{ + std::vector< SVzNLPointXYZRGBA> validPoints; + for (int line = 0; line < (int)scanLines.size(); line++) + { + for (int j = 0; j < (int)scanLines[line].size(); j++) + { + if (scanLines[line][j].pt3D.z > 1e-4) + { + SVzNLPointXYZRGBA a_pt; + double v = _F * scanLines[line][j].pt3D.y / scanLines[line][j].pt3D.z + 2000; + a_pt.nRGB = (int)(v + 0.5); + a_pt.x = scanLines[line][j].pt3D.x; + a_pt.y = scanLines[line][j].pt3D.y; + a_pt.z = scanLines[line][j].pt3D.z; + validPoints.push_back(a_pt); + } + } + } + + std::vector< SVzNLPointXYZRGBA> a_line; + SVzNLPointXYZRGBA pre_pt; + pre_pt.x = 0; + pre_pt.y = 0; + pre_pt.z = 0; + pre_pt.nRGB = -1; + for (int i = 0; i < validPoints.size(); i++) + { + if (validPoints[i].nRGB < (pre_pt.nRGB - 10)) + { + if (a_line.size() > 0) + scanData.push_back(a_line); + a_line.clear(); + } + a_line.push_back(validPoints[i]); + pre_pt = validPoints[i]; + } + if(a_line.size() > 0) + scanData.push_back(a_line); + return; +} +void _convertToGridData_XYZRGB(std::vector>& scanData, double _F, std::vector>& gridData) +{ + int lineNum = (int)scanData.size(); + int min_y = 100000000; + int max_y = -10000000; + int validStartLine = -1; + int validEndLine = -1; + for (int line = 0; line < lineNum; line++) + { + if (scanData[line].size() > 0) + { + if (validStartLine < 0) + { + validStartLine = line; + validEndLine = line; + } + else + validEndLine = line; + } + + for (int i = 0; i < (int)scanData[line].size(); i++) + { + SVzNLPointXYZRGBA& a_pt = scanData[line][i]; + if (a_pt.z > 1e-4) + { + double v = _F * a_pt.y / a_pt.z + 2000; + a_pt.nRGB = (int)(v + 0.5); + max_y = max_y < (int)a_pt.nRGB ? (int)a_pt.nRGB : max_y; + min_y = min_y > (int)a_pt.nRGB ? (int)a_pt.nRGB : min_y; + } + } + } + if (min_y == 100000000) + return; + + int vldLineNum = validEndLine - validStartLine + 1; + gridData.resize(vldLineNum); + int pt_counter = max_y - min_y + 1; + for (int i = 0; i < vldLineNum; i++) + { + gridData[i].resize(pt_counter); + for (int j = 0; j < pt_counter; j++) + { + gridData[i][j].nPointIdx = j; + gridData[i][j].pt3D.x = 0; + gridData[i][j].pt3D.y = 0; + gridData[i][j].pt3D.z = 0; + } + } + for (int line = validStartLine; line <= validEndLine; line++) + { + int gridLine = line - validStartLine; + for (int i = 0; i < (int)scanData[line].size(); i++) + { + SVzNLPointXYZRGBA& a_pt = scanData[line][i]; + if (a_pt.z > 1e-4) + { + int pt_id = a_pt.nRGB - min_y; + gridData[gridLine][pt_id].pt3D.x = a_pt.x; + gridData[gridLine][pt_id].pt3D.y = a_pt.y; + gridData[gridLine][pt_id].pt3D.z = a_pt.z; + } + } + } + return; +} + +#define ROD_POSITION_TEST_GROUP 5 +#define TEST_CONVERT_TO_GRID 0 +#define TEST_COMPUTE_CALIB_PARA 0 +#define TEST_COMPUTE_ROD_POSITION 1 void rodPositionTest(void) { +#if TEST_CONVERT_TO_GRID + //将数据转换成栅格格式格式 + char _scan_dir[256]; + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + sprintf_s(_scan_dir, "F:/ShangGu/项目/冠钦项目/矩森棒材抓取/空料框及内剩一根棒材/"); + char _scan_src_file[256]; + double _F = 1219.15; //f + for (int i = 1; i <= 3; i++) + { + //read ply txt + sprintf_s(_scan_src_file, "%sLaserDataRAW_%d.txt", _scan_dir, i); + + std::vector> scanLines; + wdReadLaserScanPointFromFile_XYZ_vector(_scan_src_file, scanLines); + + std::vector> scanData; + _removeNullData(scanLines, _F, scanData); + + std::vector> gridData; + _convertToGridData_XYZRGB(scanData, _F, gridData); + char _out_file[256]; + sprintf_s(_out_file, "%sLaserData_%d.txt", _scan_dir, i); + _outputScanDataFile(_out_file, gridData, 0.0f, 0, 0); + + printf("%s: convert done!\n", _scan_src_file); + } + printf("all converted done!\n", _scan_src_file); +#endif +#if TEST_COMPUTE_CALIB_PARA + const char* calibDataPath = "F:/ShangGu/项目/冠钦项目/矩森棒材抓取/空料框及内剩一根棒材/调平/位置1/"; + char _calib_datafile[256]; + sprintf_s(_calib_datafile, "%sGroundData_1.txt", calibDataPath); + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + std::vector> scanData; + wdReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanData); + + lineNum = (int)scanData.size(); + if (scanData.size() > 0) + { + SSG_planeCalibPara calibPara = sx_rodPosition_getBaseCalibPara(scanData); + //结果进行验证 + for (int i = 0; i < lineNum; i++) + { + if (i == 14) + int kkk = 1; + //行处理 + //调平，去除地面 + sx_rodPosition_lineDataR(scanData[i], calibPara.planeCalib, -1); + } + // + char calibFile[250]; + sprintf_s(calibFile, "%sground_calib_para.txt", calibDataPath); + _outputCalibPara(calibFile, calibPara); + char _out_file[256]; + sprintf_s(_out_file, "%sGroundData_1_calib_verify.txt", calibDataPath); + + _outputScanDataFile(_out_file, scanData, lineV, maxTimeStamp, clockPerSecond); +#if 0 + for (int fidx = fileIdx[cvtGrp].nMin; fidx <= fileIdx[cvtGrp].nMax; fidx++) + { + //fidx =4; + char _scan_file[256]; + sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[cvtGrp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + if (scanLines.size() == 0) + continue; + lineNum = (int)scanLines.size(); + for (int i = 0; i < lineNum; i++) + { + //调平，去除地面 + wd_lineDataR(scanLines[i], calibPara.planeCalib, -1); + } + sprintf_s(_scan_file, "%sLaserData_%d_calib_verify.txt", dataPath[cvtGrp], fidx); + int headNullLines = 0; + _outputScanDataFile_vector(_scan_file, scanLines, false, &headNullLines); + } +#endif + printf("%s: calib done!\n", _calib_datafile); + } +#endif + +#if TEST_COMPUTE_ROD_POSITION const char* dataPath[ROD_POSITION_TEST_GROUP] = { "F:/ShangGu/项目/冠钦项目/矩森棒材抓取/测试数据/", //0 - "F:/ShangGu/项目/冠钦项目/胶布圆棒抓取/模拟测试数据/", //1 - "F:/ShangGu/项目/冠钦项目/胶布圆棒抓取/模拟测试数据2/", //1 + "F:/ShangGu/项目/冠钦项目/矩森棒材抓取/空料框及内剩一根棒材/", //1 + "F:/ShangGu/项目/冠钦项目/矩森棒材抓取/其它尺寸棒材点云/", //2 + + "F:/ShangGu/项目/冠钦项目/胶布圆棒抓取/模拟测试数据/", //3 + "F:/ShangGu/项目/冠钦项目/胶布圆棒抓取/模拟测试数据2/", //4 }; SVzNLRange fileIdx[ROD_POSITION_TEST_GROUP] = { - {1,8}, {1,5},{1,11} + {1,8}, {1,3}, {1,31}, + {1,5},{1,11} }; const char* ver = wd_rodAndBarDetectionVersion(); printf("ver:%s\n", ver); - for (int grp = 2; grp < ROD_POSITION_TEST_GROUP; grp++) + for (int grp = 0; grp <= 2; grp++) { SSG_planeCalibPara poseCalibPara; //初始化成单位阵 @@ -1165,15 +1428,21 @@ void rodPositionTest(void) poseCalibPara.planeHeight = -1.0; for (int i = 0; i < 9; i++) poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i]; - //char calibFile[250]; - //sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); - //poseCalibPara = _readCalibPara(calibFile); + + char calibFile[250]; + sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); + poseCalibPara = _readCalibPara(calibFile); + + sprintf_s(calibFile, "%sparameter.txt", dataPath[grp]); + std::vector rodParas; + if (grp <= 2) + _readRodParas(calibFile, rodParas); for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { - //fidx =1; + //fidx =5; char _scan_file[256]; - if( (1 == grp) ||(2 == grp)) + if( (3 == grp) ||(4 == grp)) sprintf_s(_scan_file, "%s%d_LaserData_Hi229156.txt", dataPath[grp], fidx); else sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx); @@ -1188,10 +1457,9 @@ void rodPositionTest(void) long t1 = (long)GetTickCount64();//统计时间 SSX_rodParam rodParam; - if (0 == grp) + if (grp <= 2) { - rodParam.diameter = 52.0; //圆棒直径 - rodParam.len = 290; + rodParam = rodParas[fidx]; } else { @@ -1200,7 +1468,7 @@ void rodPositionTest(void) } SSG_cornerParam cornerParam; - cornerParam.cornerTh = 60; //45度角 + cornerParam.cornerTh = 45; //45度角 cornerParam.scale = rodParam.diameter / 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4; cornerParam.minEndingGap_z = 5.0; @@ -1216,8 +1484,8 @@ void rodPositionTest(void) growParam.yDeviation_max = 10.0; growParam.maxSkipDistance = 10.0; growParam.zDeviation_max = 10.0;// - growParam.minLTypeTreeLen = 100; //mm, 螺杆长度 - growParam.minVTypeTreeLen = 100; //mm + growParam.minLTypeTreeLen = rodParam.len * 0.75; //mm, 螺杆长度 + growParam.minVTypeTreeLen = rodParam.len * 0.75; //mm bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道 int errCode = 0; @@ -1233,7 +1501,7 @@ void rodPositionTest(void) &errCode); long t2 = (long)GetTickCount64(); - printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); + printf("%s: %d(ms),errCode=%d\n", _scan_file, (int)(t2 - t1),errCode); //输出测试结果 sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx); _outputRGBDScan_RGBD_rodInfo(_scan_file, scanLines, rodInfo); @@ -1241,6 +1509,7 @@ void rodPositionTest(void) _outputRodtInfo(_scan_file, rodInfo); } } +#endif } @@ -1357,8 +1626,8 @@ int main() { //ESG_testMode testMode = keSG_测试_配天螺杆定位; //ESG_testMode testMode = keSG_测试_配天定位盘定位; - ESG_testMode testMode = keSG_测试_配天新定位盘定位; - //ESG_testMode testMode = keSG_测试_棒材抓取; + //ESG_testMode testMode = keSG_测试_配天新定位盘定位; + ESG_testMode testMode = keSG_测试_棒材抓取; //ESG_testMode testMode = keSG_测试_筑裕钢筋焊缝定位; if(keSG_测试_配天螺杆定位 == testMode) diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index 82702c8..c08e9fd 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -4597,7 +4597,7 @@ void wd_getRodArcFeature_segmentPeakCornerMethod( std::vector& line_rodArcs // ) { - double arcPerPointCornerMinValue = 5; //arcÿתСֵ + double arcPerPointCornerMinValue = 5.0; //arcÿתСֵ double arcTotalCornerMinValue = 30; //ArcתСֵ std::vector segs; diff --git a/sourceCode/rodAndBarDetection.cpp b/sourceCode/rodAndBarDetection.cpp index 56fe5d0..f7009fa 100644 --- a/sourceCode/rodAndBarDetection.cpp +++ b/sourceCode/rodAndBarDetection.cpp @@ -22,7 +22,8 @@ //version 1.2.11 : ֳݵ //version 1.3.0 : µĶλIJ //version 1.3.1 : λIJ -std::string m_strVersion = "1.3. "; +//version 1.3.2 : ɭץȡһ뷵ش󣬲˵ƽAPI +std::string m_strVersion = "1.3.2"; const char* wd_rodAndBarDetectionVersion(void) { return m_strVersion.c_str(); @@ -245,7 +246,7 @@ void rodAarcFeatueDetection( int linePtNum = (int)scanLines[0].size(); for (int line = 0; line < lineNum; line++) { - if (line == 1304) + if (line == 413) int kkk = 1; std::vector& lineData = scanLines[line]; @@ -2676,7 +2677,7 @@ void sx_rodPositioning( if ((rodArcTrees_v.size() == 0) && (rodArcTrees_h.size() == 0)) { - *errCode = SG_ERR_NOT_GRID_FORMAT; + *errCode = SX_ERR_ZERO_OBJECTS; return; } diff --git a/sourceCode/rodAndBarDetection_Export.h b/sourceCode/rodAndBarDetection_Export.h index 68dbda3..139d604 100644 --- a/sourceCode/rodAndBarDetection_Export.h +++ b/sourceCode/rodAndBarDetection_Export.h @@ -65,7 +65,7 @@ SG_APISHARED_EXPORT const char* wd_rodAndBarDetectionVersion(void); SG_APISHARED_EXPORT SSG_planeCalibPara sx_rodPosition_getBaseCalibPara( std::vector< std::vector>& scanLines); -#if 0 +#if 1 //̬ƽȥ SG_APISHARED_EXPORT void sx_rodPosition_lineDataR( std::vector< SVzNL3DPosition>& a_line,