diff --git a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp index 477d914..fc494f1 100644 --- a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp +++ b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp @@ -501,7 +501,7 @@ void _outputRGBDScanLapWeld_RGBD( sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; } - if (debugData) + if ((debugData) && (workpieceCorner.workpieceType > 0)) { int linePtNum = debugData[0].edge_size + debugData[0].edgeLink1_size + debugData[0].edgeLink2_size; linePtNum += debugData[1].edge_size + debugData[1].edgeLink1_size + debugData[1].edgeLink2_size; @@ -606,15 +606,20 @@ void _outputRGBDScanLapWeld_RGBD( #define CONVERT_TO_GRID 0 #define TEST_COMPUTE_CALIB_PARA 0 #define TEST_COMPUTE_CORNER 1 -#define TEST_GROUP 1 +#define TEST_GROUP 6 int main() { const char* dataPath[TEST_GROUP] = { - "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\" //0 + "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\", //0 + "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度1\\", //1 + "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度2\\", //2 + "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度3\\", //3 + "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度4\\", //4 + "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\", //5 }; SVzNLRange fileIdx[TEST_GROUP] = { - {1,3} + {1,3}, {6,8}, {6,8}, {6,8}, {6,8}, {9,12} }; #if CONVERT_TO_GRID @@ -641,7 +646,7 @@ int main() #if TEST_COMPUTE_CALIB_PARA char _calib_datafile[256]; - sprintf_s(_calib_datafile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\scanData_1.txt"); + sprintf_s(_calib_datafile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\9_LazerData_Hi229229.txt"); int lineNum = 0; float lineV = 0.0f; int dataCalib = 0; @@ -677,10 +682,10 @@ int main() } // char calibFile[250]; - sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\ground_calib_para.txt"); + sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\ground_calib_para.txt"); _outputCalibPara(calibFile, calibPara); char _out_file[256]; - sprintf_s(_out_file, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\scanData_ground_1_calib.txt"); + sprintf_s(_out_file, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\scanData_ground_1_calib.txt"); int headNullLines = 0; _outputScanDataFile_vector(_out_file, scanData, false, &headNullLines); printf("%s: calib done!\n", _calib_datafile); @@ -688,7 +693,10 @@ int main() #endif #if TEST_COMPUTE_CORNER - for (int grp = 0; grp <= 0; grp++) + const char* ver = wd_BQWorkpieceCornerVersion(); + printf("ver:%s\n", ver); + + for (int grp = 1; grp <= 5; grp++) { SSG_planeCalibPara poseCalibPara; //初始化成单位阵 @@ -710,12 +718,40 @@ int main() sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\ground_calib_para.txt"); poseCalibPara = _readCalibPara(calibFile); } + else if (grp == 1) + { + sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度1\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } + else if (grp == 2) + { + sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度2\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } + else if (grp == 3) + { + sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度3\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } + else if (grp == 4) + { + sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度4\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } + else if (grp == 5) + { + sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { //fidx =1; char _scan_file[256]; - sprintf_s(_scan_file, "%sscanData_%d_grid.txt", dataPath[grp], fidx); + if(0 == grp) + sprintf_s(_scan_file, "%sscanData_%d_grid.txt", dataPath[grp], fidx); + else + sprintf_s(_scan_file, "%s%d_LazerData_Hi229229.txt", dataPath[grp], fidx); std::vector> scanLines; vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); diff --git a/QRcode3Ddetection_test/QRcode3Ddetection_test.cpp b/QRcode3Ddetection_test/QRcode3Ddetection_test.cpp index 4e384c3..15cb830 100644 --- a/QRcode3Ddetection_test/QRcode3Ddetection_test.cpp +++ b/QRcode3Ddetection_test/QRcode3Ddetection_test.cpp @@ -73,6 +73,95 @@ void vzReadLaserScanPointFromFile_plyTxt(const char* fileName, std::vector< SVzN return; } +typedef struct +{ + int x; + double y; + double z; +} WD_Encode3DPoint; +void vzReadLaserScanPointFromFile_encodePlyTxt(const char* fileName, std::vector< WD_Encode3DPoint>& scanData) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + while (std::getline(inputFile, linedata)) + { + if (linedata.empty()) + continue; + + int X; + double Y, Z, tmp; + sscanf_s(linedata.c_str(), "%d,%lf,%lf", &X, &Y, &Z); + WD_Encode3DPoint a_pt; + a_pt.x = X; + a_pt.y = Y; + a_pt.z = Z; + scanData.push_back(a_pt); + } + return; +} +void convertEncodePlyToVzData(std::vector< WD_Encode3DPoint>& scanData, std::vector>& scanLines, double stepping, bool toGrid) +{ + int size = (int)scanData.size(); + int preEnc = -1; + std::vector a_line; + int vldNum = 0; + double lineX = 0; + for (int i = 0; i < size; i++) + { + WD_Encode3DPoint a_ply = scanData[i]; + if (a_ply.x != preEnc) //new line + { + if (vldNum > 0) + { + scanLines.push_back(a_line); + a_line.clear(); + } + if (preEnc < 0) + lineX = 0; + else + lineX += (double)(a_ply.x - preEnc) * stepping; + preEnc = a_ply.x; + vldNum = 0; + } + + SVzNL3DPosition a_pt; + a_pt.nPointIdx = vldNum; + + bool zeroPt = true; + if ((a_ply.z > 29.0) && (a_ply.z < 34.0)) + { + a_pt.pt3D.x = lineX; + a_pt.pt3D.y = a_ply.y; + a_pt.pt3D.z = a_ply.z; + zeroPt = false; + } + else + { + a_pt.pt3D.x = 0; + a_pt.pt3D.y = 0; + a_pt.pt3D.z = 0; + } + if (toGrid == true) + { + a_line.push_back(a_pt); + vldNum++; + } + else + { + if (false == zeroPt) + { + a_line.push_back(a_pt); + vldNum++; + } + } + + } +} + void _outputScanDataFile_vector(char* fileName, std::vector>& scanLines, bool removeZeros, int* headNullLines) { std::ofstream sw(fileName); @@ -133,6 +222,46 @@ void _outputScanDataFile_vector(char* fileName, std::vector>& scanLines, bool removeZeros, int* headNullLines) +{ + std::ofstream sw(fileName); + int lineNum = scanLines.size(); + if (lineNum == 0) + return; + + int null_lines = 0; + bool counterNull = true; + for (int line = 0; line < lineNum; line++) + { + int linePtNum = scanLines[line].size(); + if (linePtNum == 0) + continue; + + bool isNull = true; + for (int i = 0; i < linePtNum; i++) + { + SVzNL3DPoint* pt3D = &scanLines[line][i].pt3D; + if ((pt3D->z > 1e-4) && (isNull == true)) + isNull = false; + if ((true == removeZeros) && (pt3D->z < 1e-4)) + continue; + float x = (float)pt3D->x; + float y = (float)pt3D->y; + float z = (float)pt3D->z; + sw << x << "," << y << "," << z<< std::endl; + } + if (true == counterNull) + { + if (true == isNull) + null_lines++; + else + counterNull = false; + } + } + *headNullLines = null_lines; + sw.close(); +} + void _outputRGBDScanDataFile_RGBD( char* fileName, std::vector>& scanLines, @@ -562,20 +691,53 @@ void _getRoiClouds( return; } +#define CONVERT_TO_GRID 1 #define TEST_COMPUTE_CALIB_PARA 0 -#define TEST_COMPUTE_QRCODE_IMG 1 -#define TEST_GROUP 2 +#define TEST_COMPUTE_QRCODE_IMG 0 +#define TEST_GROUP 2 int main() { const char* dataPath[TEST_GROUP] = { - "F:\\ShangGu\\项目\\工件端部圆点二维码\\", //0 - "F:\\ShangGu\\项目\\工件端部圆点二维码\\字符\\" //1 + "F:\\ShangGu\\项目\\国铭铸管\\二维码\\", //0 + "F:\\ShangGu\\项目\\国铭铸管\\字符\\" //1 }; SVzNLRange fileIdx[TEST_GROUP] = { {3,3}, {1,8} }; +#if CONVERT_TO_GRID + int convertGrp = 1; + for (int fidx = fileIdx[convertGrp].nMin; fidx <= fileIdx[convertGrp].nMax; fidx++) + { + char _scan_file[256]; + sprintf_s(_scan_file, "%sdata%d.txt", dataPath[convertGrp], fidx); + + std::vector< WD_Encode3DPoint> scanPly; + vzReadLaserScanPointFromFile_encodePlyTxt(_scan_file, scanPly); + + double stepping = 0.0625; + std::vector> scanData; + std::vector> gridScanData; + bool toGrid = false; + convertEncodePlyToVzData(scanPly, scanData, stepping, toGrid); + toGrid = true; + convertEncodePlyToVzData(scanPly, gridScanData, stepping, toGrid); + //将数据恢复为按扫描线存储格式 + sprintf_s(_scan_file, "%sscanDataPlyTxt_%d.txt", dataPath[convertGrp], fidx); + int headNullLines = 0; + _outputScanPlyTxtFile_vector(_scan_file, scanData, false, &headNullLines); +#if 0 + sprintf_s(_scan_file, "%svzScanData_%d.txt", dataPath[convertGrp], fidx); + int headNullLines = 0; + _outputScanDataFile_vector(_scan_file, scanData, false, &headNullLines); + sprintf_s(_scan_file, "%svzScanData_grid_%d.txt", dataPath[convertGrp], fidx); + _outputScanDataFile_vector(_scan_file, gridScanData, false, &headNullLines); + printf("%s: head null lines = %d\n", _scan_file, headNullLines); +#endif + } +#endif + #if TEST_COMPUTE_CALIB_PARA char _calib_datafile[256]; sprintf_s(_calib_datafile, "F:\\ShangGu\\项目\\工件端部圆点二维码\\LaserLine3_grid.txt"); diff --git a/SG_Algorithm.sln b/SG_Algorithm.sln index 65b4703..5957761 100644 --- a/SG_Algorithm.sln +++ b/SG_Algorithm.sln @@ -118,6 +118,16 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "BQ_workpieceCornerExtract_t {AD8415B7-A745-4184-87B8-95619E5066D6} = {AD8415B7-A745-4184-87B8-95619E5066D6} EndProjectSection EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "particleSizeMeasurement_test", "particleSizeMeasurement_test\particleSizeMeasurement_test.vcxproj", "{0F4995A6-3978-4EF6-87AD-CC15EC9B007B}" + ProjectSection(ProjectDependencies) = postProject + {8FD0F574-61C6-4094-8521-33AEDDB44797} = {8FD0F574-61C6-4094-8521-33AEDDB44797} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "particleSizeMeasurement", "particleSizeMeasurement\particleSizeMeasurement.vcxproj", "{8FD0F574-61C6-4094-8521-33AEDDB44797}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + EndProjectSection +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -310,6 +320,22 @@ Global {CF563709-0402-447E-BFCC-7701CC90D0AF}.Release|x64.Build.0 = Release|x64 {CF563709-0402-447E-BFCC-7701CC90D0AF}.Release|x86.ActiveCfg = Release|Win32 {CF563709-0402-447E-BFCC-7701CC90D0AF}.Release|x86.Build.0 = Release|Win32 + {0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Debug|x64.ActiveCfg = Debug|x64 + {0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Debug|x64.Build.0 = Debug|x64 + {0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Debug|x86.ActiveCfg = Debug|Win32 + {0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Debug|x86.Build.0 = Debug|Win32 + {0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Release|x64.ActiveCfg = Release|x64 + {0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Release|x64.Build.0 = Release|x64 + {0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Release|x86.ActiveCfg = Release|Win32 + {0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Release|x86.Build.0 = Release|Win32 + {8FD0F574-61C6-4094-8521-33AEDDB44797}.Debug|x64.ActiveCfg = Debug|x64 + {8FD0F574-61C6-4094-8521-33AEDDB44797}.Debug|x64.Build.0 = Debug|x64 + {8FD0F574-61C6-4094-8521-33AEDDB44797}.Debug|x86.ActiveCfg = Debug|Win32 + {8FD0F574-61C6-4094-8521-33AEDDB44797}.Debug|x86.Build.0 = Debug|Win32 + {8FD0F574-61C6-4094-8521-33AEDDB44797}.Release|x64.ActiveCfg = Release|x64 + {8FD0F574-61C6-4094-8521-33AEDDB44797}.Release|x64.Build.0 = Release|x64 + {8FD0F574-61C6-4094-8521-33AEDDB44797}.Release|x86.ActiveCfg = Release|Win32 + {8FD0F574-61C6-4094-8521-33AEDDB44797}.Release|x86.Build.0 = Release|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/bagPositioning_test/bagPositioning_test.cpp b/bagPositioning_test/bagPositioning_test.cpp index c34b771..32af194 100644 --- a/bagPositioning_test/bagPositioning_test.cpp +++ b/bagPositioning_test/bagPositioning_test.cpp @@ -2868,8 +2868,8 @@ int main() else if (grp == 19) //纸箱拆垛数据\vizum数据 { algoParam.bagParam.bagL = 500;// 500; // 420; //袋子长65cm - algoParam.bagParam.bagW = 320; // 420; // 320; //袋子宽40cm - algoParam.bagParam.bagH = 70; //袋子高16cm + algoParam.bagParam.bagW = 350; // 420; // 320; //袋子宽40cm + algoParam.bagParam.bagH = 80; //袋子高16cm algoParam.growParam.maxLineSkipNum = 5; algoParam.growParam.yDeviation_max = 20.0; algoParam.growParam.maxSkipDistance = 20.0; @@ -3036,7 +3036,7 @@ int main() //fidx = 193; if (grp < TEST_TOP_VIEW_GROUP) //正面抓取 { - algoParam.supportRotate = 1; //支持相机与对象之间有一个旋转角度（小于30度） + algoParam.supportRotate = 0; //支持相机与对象之间有一个旋转角度（小于30度） int lineNum = 0; float lineV = 0.0f; diff --git a/baseAlgorithm/baseAlgorithm.vcxproj b/baseAlgorithm/baseAlgorithm.vcxproj index 84eca3c..f858582 100644 --- a/baseAlgorithm/baseAlgorithm.vcxproj +++ b/baseAlgorithm/baseAlgorithm.vcxproj @@ -173,6 +173,7 @@ + diff --git a/sourceCode/BQ_workpieceCornerExtraction.cpp b/sourceCode/BQ_workpieceCornerExtraction.cpp index 4744e53..efa1f23 100644 --- a/sourceCode/BQ_workpieceCornerExtraction.cpp +++ b/sourceCode/BQ_workpieceCornerExtraction.cpp @@ -5,6 +5,12 @@ #include #include +std::string m_strVersion = "1.1.0"; +const char* wd_BQWorkpieceCornerVersion(void) +{ + return m_strVersion.c_str(); +} + //һƽƽ //пһƽͲοƽƽ棬ߵƽеƽ //תΪƽƽ淨ΪֱIJ @@ -154,6 +160,27 @@ typedef struct SVzNL3DPoint edge_link2_ends[2]; }SSX_featureContour; +typedef struct +{ + int lineIdx; + int ptIdx; + double R; + double angle; + double x; + double y; + double z; +}SWD_polarPt; + +//ʱתʱ > 0 ˳ʱתʱ < 0 +cv::Point2f _rotate2D(cv::Point2f pt, double sinTheta, double cosTheta) +{ + return (cv::Point2f((float)(pt.x * cosTheta - pt.y * sinTheta), (float)(pt.x * sinTheta + pt.y * cosTheta))); +} + +bool compareByAngle(const SWD_polarPt& a, const SWD_polarPt& b) { + return a.angle < b.angle; +} + SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( std::vector< std::vector>& scanLines, const SSG_cornerParam cornerPara, @@ -179,328 +206,763 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( int linePtNum = (int)scanLines[0].size(); bool isGridData = true; - //ֱȡ - std::vector> jumpFeatures_v; - for (int line = 0; line < lineNum; line++) - { - if (line == 202) - int kkk = 1; - - std::vector& lineData = scanLines[line]; - if (linePtNum != (int)lineData.size()) - isGridData = false; - - //˲˳쳣 - sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam); - - std::vector line_features; - int dataSize = (int)lineData.size(); - sg_getLineCornerFeature_BQ( - &lineData[0], - dataSize, - line, - groundCalibPara.planeHeight, - cornerPara, //scaleͨȡbagH1/4 - line_features); - jumpFeatures_v.push_back(line_features); - } - - if (false == isGridData)//ݲʽ - { - *errCode = SG_ERR_NOT_GRID_FORMAT; - return workpieceCorners; - } - - //ˮƽɨ - std::vector> hLines; - hLines.resize(linePtNum); - for (int i = 0; i < linePtNum; i++) - hLines[i].resize(lineNum); - for (int line = 0; line < lineNum; line++) - { - for (int j = 0; j < linePtNum; j++) - { - scanLines[line][j].nPointIdx = 0; //ԭʼݵ0תʹã - hLines[j][line] = scanLines[line][j]; - hLines[j][line].pt3D.x = scanLines[line][j].pt3D.y; - hLines[j][line].pt3D.y = scanLines[line][j].pt3D.x; - } - } - //ˮƽarcȡ - std::vector> jumpFeatures_h; - int lineNum_h = (int)hLines.size(); - for (int line = 0; line < lineNum_h; line++) - { - if (line == 416) - int kkk = 1; - std::vector& lineData = hLines[line]; - //˲˳쳣 - int ptNum = (int)lineData.size(); - sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam); - - std::vector line_features; - int dataSize = (int)lineData.size(); - sg_getLineCornerFeature_BQ( - &hLines[line][0], - dataSize, - line, - groundCalibPara.planeHeight, - cornerPara, //scaleͨȡbagH1/4 - line_features); - jumpFeatures_h.push_back(line_features); - } - - // - //ֱ߷ - std::vector v_trees; - for (int line = 0; line < lineNum; line++) - { - bool isLastLine = false; - if (line == lineNum - 1) - isLastLine = true; - std::vector& a_lineJumpFeature = jumpFeatures_v[line]; - if (a_lineJumpFeature.size() > 0) - int kkk = 1; - if (line == 202) - int kkk = 1; - sg_lineFeaturesGrowing( - line, - isLastLine, - a_lineJumpFeature, - v_trees, - growParam); - } - - //ˮƽɨ˶ - std::vector h_trees; - for (int line = 0; line < lineNum_h; line++) - { - if (line == 650) - int kkk = 1; - bool isLastLine = false; - if (line == lineNum_h - 1) - isLastLine = true; - std::vector& a_lineJumpFeature = jumpFeatures_h[line]; - sg_lineFeaturesGrowing( - line, - isLastLine, - a_lineJumpFeature, - h_trees, - growParam); - } - - //treeϢ - std::vector allTreesInfo; //߽ - SSG_treeInfo a_nullTree; - memset(&a_nullTree, 0, sizeof(SSG_treeInfo)); - allTreesInfo.push_back(a_nullTree); //ִ洢λtreeIdxͬλã - //ǣбע - int hvTreeIdx = 1; - for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++) - { - SSG_featureTree* a_vTree = &v_trees[i]; - sg_getTreeROI(a_vTree); - //¼TreeϢ - SSG_treeInfo a_treeInfo; - a_treeInfo.vTreeFlag = 1; - a_treeInfo.treeIdx = hvTreeIdx; - a_treeInfo.treeType = a_vTree->treeType; - a_treeInfo.sLineIdx = a_vTree->sLineIdx; - a_treeInfo.eLineIdx = a_vTree->eLineIdx; - a_treeInfo.roi = a_vTree->roi; - allTreesInfo.push_back(a_treeInfo); - - std::vector a_weld_contour; - //ԭʼϱǣͬʱMaskϱ - for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++) - { - SSG_basicFeature1D* a_feature = &a_vTree->treeNodes[j]; - if (scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.z > 1e-4)//Ŀ˺0 - { - int existEdgeId = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx >> 16; - if (existEdgeId == 0) - { - scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType; - scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx &= 0xffff; - scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx += hvTreeIdx << 16; - } - a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D); - } - } - hvTreeIdx++; - } - int hTreeStart = hvTreeIdx; - ////ע:ˮƽ - for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++) - { - SSG_featureTree* a_hTree = &h_trees[i]; - sg_getTreeROI(a_hTree); - //¼TreeϢ - SSG_treeInfo a_treeInfo; - a_treeInfo.vTreeFlag = 0; - a_treeInfo.treeIdx = hvTreeIdx; - a_treeInfo.treeType = a_hTree->treeType; - a_treeInfo.sLineIdx = a_hTree->sLineIdx; - a_treeInfo.eLineIdx = a_hTree->eLineIdx; - a_treeInfo.roi.left = a_hTree->roi.top; //ˮƽɨxyǽ - a_treeInfo.roi.right = a_hTree->roi.bottom; - a_treeInfo.roi.top = a_hTree->roi.left; - a_treeInfo.roi.bottom = a_hTree->roi.right; - allTreesInfo.push_back(a_treeInfo); - - std::vector a_weld_contour; - //ԭʼϱǣͬʱMaskϱ - for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++) - { - SSG_basicFeature1D* a_feature = &a_hTree->treeNodes[j]; - if (scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.z > 1e-4)//Ŀ˺0 - { - int existEdgeId = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx >> 16; - if (existEdgeId == 0) - { - scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4; - scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx &= 0xffff; - scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += hvTreeIdx << 16; - } - a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D); - } - } - hvTreeIdx++; - } - int hvTreeSize = hvTreeIdx; - - if(v_trees.size() < 2) - { - *errCode = SX_ERR_INVLD_VTREE_NUM; - return workpieceCorners; - } - if (h_trees.size() < 2) - { - *errCode = SX_ERR_INVLD_HTREE_NUM; - return workpieceCorners; - } - - //ѰvTreeϺ - int vTree_T = 0; - int vTree_B = 0; - for (int i = 1, i_max = (int)v_trees.size(); i < i_max; i++) - { - if (v_trees[i].roi.top < v_trees[vTree_T].roi.top) - vTree_T = i; - if (v_trees[i].roi.bottom > v_trees[vTree_B].roi.bottom) - vTree_B = i; - } - - //ѰhTree - int hTree_L = 0; - int hTree_R = 0; - for (int i = 1, i_max = (int)h_trees.size(); i < i_max; i++) - { - //ˮƽɨxyǽģҶӦROItopBottom - if (h_trees[i].roi.top < h_trees[hTree_L].roi.top) - hTree_L = i; - if (h_trees[i].roi.bottom > h_trees[hTree_R].roi.bottom) - hTree_R = i; - } - SSX_featureContour region[4]; - region[0].rgnIdx = 0; //Left - _getEdgeContour(&h_trees[hTree_L], region[0].edge, scanLines,false); - //ѰҶӦ - SVzNL3DPoint firstPt = region[0].edge[0]; - SVzNL3DPoint lastPt = region[0].edge.back(); - int idx0 = _getPointClosestContour(v_trees, true, firstPt, scanLines, true); - if (idx0 < 0) + //ӦתǶ { - *errCode = SX_ERR_INVLD_CLOSES_PT; - return workpieceCorners; - } - _getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[0].edgeLink_1, scanLines, true, workpieceParam.lineLen); - int idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, true); - if (idx1 < 0) - { - *errCode = SX_ERR_INVLD_CLOSES_PT; - return workpieceCorners; - } - _getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[0].edgeLink_2, scanLines, true, workpieceParam.lineLen); - if ((region[0].edgeLink_1.size() < 5) || (region[0].edgeLink_2.size() < 5)) - { - *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; - return workpieceCorners; + //ֱȡ + std::vector> jumpFeatures_v_raw; + for (int line = 0; line < lineNum; line++) + { + if (line == 202) + int kkk = 1; + + std::vector& lineData = scanLines[line]; + if (linePtNum != (int)lineData.size()) + isGridData = false; + + //˲˳쳣 + sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam); + + std::vector line_features; + int dataSize = (int)lineData.size(); + sg_getLineCornerFeature_BQ( + &lineData[0], + dataSize, + line, + groundCalibPara.planeHeight, + cornerPara, //scaleͨȡbagH1/4 + line_features); + jumpFeatures_v_raw.push_back(line_features); + } + + if (false == isGridData)//ݲʽ + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return workpieceCorners; + } + + //ˮƽɨ + std::vector> hLines_raw; + hLines_raw.resize(linePtNum); + for (int i = 0; i < linePtNum; i++) + hLines_raw[i].resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + scanLines[line][j].nPointIdx = 0; //ԭʼݵ0תʹã + hLines_raw[j][line] = scanLines[line][j]; + hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y; + hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x; + } + } + //ˮƽarcȡ + std::vector> jumpFeatures_h_raw; + int lineNum_h_raw = (int)hLines_raw.size(); + for (int line = 0; line < lineNum_h_raw; line++) + { + if (line == 416) + int kkk = 1; + std::vector& lineData = hLines_raw[line]; + //˲˳쳣 + int ptNum = (int)lineData.size(); + sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam); + + std::vector line_features; + int dataSize = (int)lineData.size(); + sg_getLineCornerFeature_BQ( + &hLines_raw[line][0], + dataSize, + line, + groundCalibPara.planeHeight, + cornerPara, //scaleͨȡbagH1/4 + line_features); + jumpFeatures_h_raw.push_back(line_features); + } + + //˳ + //ֱ߷ + std::vector v_trees; + for (int line = 0; line < lineNum; line++) + { + bool isLastLine = false; + if (line == lineNum - 1) + isLastLine = true; + std::vector& a_lineJumpFeature = jumpFeatures_v_raw[line]; + if (a_lineJumpFeature.size() > 0) + int kkk = 1; + if (line == 202) + int kkk = 1; + sg_lineFeaturesGrowing( + line, + isLastLine, + a_lineJumpFeature, + v_trees, + growParam); + } + + //ˮƽɨ˶ + std::vector h_trees; + for (int line = 0; line < lineNum_h_raw; line++) + { + if (line == 650) + int kkk = 1; + bool isLastLine = false; + if (line == lineNum_h_raw - 1) + isLastLine = true; + std::vector& a_lineJumpFeature = jumpFeatures_h_raw[line]; + sg_lineFeaturesGrowing( + line, + isLastLine, + a_lineJumpFeature, + h_trees, + growParam); + } + + std::vector polarPoints; +#if 0 + for (int line = 0; line < lineNum; line++) + { + std::vector& a_lineJumpFeature = jumpFeatures_v_raw[line]; + for (int pi = 0, pi_max = (int)a_lineJumpFeature.size(); pi < pi_max; pi++) + { + int lineIdx = a_lineJumpFeature[pi].jumpPos2D.x; + int ptIdx = a_lineJumpFeature[pi].jumpPos2D.y; + if (scanLines[lineIdx][ptIdx].nPointIdx >= 0) + { + SWD_polarPt a_polarPt; + a_polarPt.lineIdx = lineIdx; + a_polarPt.ptIdx = ptIdx; + a_polarPt.R = 0; + a_polarPt.angle = 0; + a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x; + a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y; + a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z; + polarPoints.push_back(a_polarPt); + scanLines[lineIdx][ptIdx].nPointIdx = -1; + } + } + } + for (int line = 0; line < lineNum_h_raw; line++) + { + std::vector& a_lineJumpFeature = jumpFeatures_h_raw[line]; + for (int pi = 0, pi_max = (int)a_lineJumpFeature.size(); pi < pi_max; pi++) + { + int lineIdx = a_lineJumpFeature[pi].jumpPos2D.y; + int ptIdx = a_lineJumpFeature[pi].jumpPos2D.x; + if (scanLines[lineIdx][ptIdx].nPointIdx >= 0) + { + SWD_polarPt a_polarPt; + a_polarPt.lineIdx = lineIdx; + a_polarPt.ptIdx = ptIdx; + a_polarPt.R = 0; + a_polarPt.angle = 0; + a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x; + a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y; + a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z; + polarPoints.push_back(a_polarPt); + scanLines[lineIdx][ptIdx].nPointIdx = -1; + } + } + } +#else + for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++) + { + SSG_featureTree* a_vTree = &v_trees[i]; + //ԭʼϱǣͬʱMaskϱ + for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++) + { + int lineIdx = a_vTree->treeNodes[j].jumpPos2D.x; + int ptIdx = a_vTree->treeNodes[j].jumpPos2D.y; + if (scanLines[lineIdx][ptIdx].nPointIdx >= 0) + { + SWD_polarPt a_polarPt; + a_polarPt.lineIdx = lineIdx; + a_polarPt.ptIdx = ptIdx; + a_polarPt.R = 0; + a_polarPt.angle = 0; + a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x; + a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y; + a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z; + polarPoints.push_back(a_polarPt); + scanLines[lineIdx][ptIdx].nPointIdx = -1; + } + } + } + for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++) + { + SSG_featureTree* a_hTree = &h_trees[i]; + //ԭʼϱǣͬʱMaskϱ + for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++) + { + int lineIdx = a_hTree->treeNodes[j].jumpPos2D.y; + int ptIdx = a_hTree->treeNodes[j].jumpPos2D.x; + if (scanLines[lineIdx][ptIdx].nPointIdx >= 0) + { + SWD_polarPt a_polarPt; + a_polarPt.lineIdx = lineIdx; + a_polarPt.ptIdx = ptIdx; + a_polarPt.R = 0; + a_polarPt.angle = 0; + a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x; + a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y; + a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z; + polarPoints.push_back(a_polarPt); + scanLines[lineIdx][ptIdx].nPointIdx = -1; + } + } + } + +#endif + //㼸 + int contourPtSize = (int)polarPoints.size(); + if (contourPtSize == 0) + { + *errCode = SX_ERR_ZERO_CONTOUR_PT; + return workpieceCorners; + } + double center_x = 0; + double center_y = 0; + for (int pi = 0; pi < contourPtSize; pi++) + { + center_x += polarPoints[pi].x; + center_y += polarPoints[pi].y; + } + center_x = center_x / (double)contourPtSize; + center_y = center_y / (double)contourPtSize; + //㼫RTheta + for (int pi = 0; pi < contourPtSize; pi++) + { + double angle = atan2(polarPoints[pi].y - center_y, polarPoints[pi].x - center_x); + angle = (angle / PI) * 180 +180.0; + double R = sqrt(pow(polarPoints[pi].y - center_y, 2) + pow(polarPoints[pi].x - center_x, 2)); + polarPoints[pi].R = R; + polarPoints[pi].angle = angle; + } + //ǶȴС + std::sort(polarPoints.begin(), polarPoints.end(), compareByAngle); + //ȡRֵ + std::vector rPeaks; + std::vector polarRPeakPts; + int winSize = contourPtSize / 36; //+-10ȷΧ + if (winSize < 5) + winSize = 5; + for (int pi = 0; pi < contourPtSize; pi++) + { + double currR = polarPoints[pi].R; + bool isPeak = true; + for (int k = -winSize; k <= winSize; k++) + { + int idx = (pi + k + contourPtSize) % contourPtSize; //Ͳνṹ + if (polarPoints[idx].R > currR) + { + isPeak = false; + break; + } + } + if (true == isPeak) + { + rPeaks.push_back(pi); + polarRPeakPts.push_back(polarPoints[pi]); + } + } + if (rPeaks.size() != 8) + { + *errCode = SX_ERR_INVLID_RPEAK_NUM; + return workpieceCorners; + } + //SSX_featureContour region[4]; + //Left, top, right, bottom + SSG_intPair peakPair[4]; + int pairIdx = 0; + for (int i = 0; i < 8; i++) + { + if (polarRPeakPts[i].lineIdx < 0) + continue; + //ǰĿ + int pre_idx = (i - 1 + 8) % 8; + int nxt_idx = (i + 1 + 8) % 8; + bool pre_isPair = false; + if (rPeaks[pre_idx] >= 0) + { + int mid_idx; + if (rPeaks[i] < rPeaks[pre_idx]) + mid_idx = ((contourPtSize + rPeaks[i] + rPeaks[pre_idx]) / 2) % contourPtSize; + else + mid_idx = (rPeaks[i] + rPeaks[pre_idx]) / 2; + double mid_R = polarPoints[mid_idx].R; + cv::Point2f midChord = cv::Point2f((polarRPeakPts[i].x + polarRPeakPts[pre_idx].x) / 2, (polarRPeakPts[i].y + polarRPeakPts[pre_idx].y) / 2); + double meanR = sqrt(pow(midChord.x - center_x,2) + pow(midChord.y-center_y,2)); + double delta_R = abs(mid_R - meanR); + if (delta_R < 10) + pre_isPair = true; + } + bool nxt_isPair = false; + if (rPeaks[nxt_idx] >= 0) + { + int mid_idx; + if (rPeaks[nxt_idx] < rPeaks[i]) + mid_idx = ((contourPtSize + rPeaks[nxt_idx] + rPeaks[i]) / 2) % contourPtSize; + else + mid_idx = (rPeaks[i] + rPeaks[nxt_idx]) / 2; + double mid_R = polarPoints[mid_idx].R; + cv::Point2f midChord = cv::Point2f((polarRPeakPts[i].x + polarRPeakPts[nxt_idx].x) / 2, (polarRPeakPts[i].y + polarRPeakPts[nxt_idx].y) / 2); + double meanR = sqrt(pow(midChord.x - center_x, 2) + pow(midChord.y - center_y, 2)); + double delta_R = abs(mid_R - meanR); + if (delta_R < 10) + nxt_isPair = true; + } + + if ((true == pre_isPair) && (false == nxt_isPair)) + { + peakPair[pairIdx].idx = pairIdx; + peakPair[pairIdx].data_0 = pre_idx; + peakPair[pairIdx].data_1 = i; + pairIdx++; + polarRPeakPts[pre_idx].lineIdx = -1; + polarRPeakPts[i].lineIdx = -1; + } + else if ((false == pre_isPair) && (true == nxt_isPair)) + { + peakPair[pairIdx].idx = pairIdx; + peakPair[pairIdx].data_0 = i; + peakPair[pairIdx].data_1 = nxt_idx; + pairIdx++; + polarRPeakPts[nxt_idx].lineIdx = -1; + polarRPeakPts[i].lineIdx = -1; + } + else + { + *errCode = SX_ERR_INVLID_RPEAK_PAIR; + return workpieceCorners; + } + } + //SSX_featureContour region[4] + double centerAngle; + int idx0 = peakPair[0].data_0; + int idx1 = peakPair[0].data_1; + if (polarRPeakPts[idx0].angle > polarRPeakPts[idx1].angle) + { + centerAngle = (360 + polarRPeakPts[idx0].angle + polarRPeakPts[idx1].angle) / 2; + if (centerAngle >= 360) + centerAngle = centerAngle - 360; + } + else + centerAngle = (polarRPeakPts[idx0].angle + polarRPeakPts[idx1].angle) / 2; + int LRTB[4]; + if ((centerAngle > 315) || (centerAngle <= 45)) //Left + { + LRTB[0] = 0; //left + LRTB[1] = 1; //top + LRTB[2] = 2; //right + LRTB[3] = 3; //bottom + } + else if ((centerAngle > 45) && (centerAngle <= 135)) //Top + { + LRTB[0] = 1; //top + LRTB[1] = 2; //right + LRTB[2] = 3; //bottom + LRTB[3] = 0; //left + } + else if ((centerAngle > 135) && (centerAngle <= 225)) //Right + { + LRTB[0] = 2; //right + LRTB[1] = 3; //bottom + LRTB[2] = 0; //left + LRTB[3] = 1; //top + } + else //Bottom + { + LRTB[0] = 3; //bottom + LRTB[1] = 0; //left + LRTB[2] = 1; //top + LRTB[3] = 2; //right + } + + for (int i = 0; i < 4; i++) + { + int rgnIdx = LRTB[i]; + int idx_0 = peakPair[i].data_0; + int idx_1 = peakPair[i].data_1; + std::vector edge; + std::vector edge_link1; + std::vector edge_link2; + int startIdx = rPeaks[idx_0]; + int endIdx = rPeaks[idx_1]; + if (startIdx < endIdx) + { + for (int m = startIdx + 1; m < endIdx; m++) + { + SVzNL3DPoint a_pt = { polarPoints[m].x, polarPoints[m].y, polarPoints[m].z }; + edge.push_back(a_pt); + } + } + else // + { + for (int m = startIdx + 1; m < contourPtSize; m++) + { + SVzNL3DPoint a_pt = { polarPoints[m].x, polarPoints[m].y, polarPoints[m].z }; + edge.push_back(a_pt); + } + for (int m = 0; m < endIdx; m++) + { + SVzNL3DPoint a_pt = { polarPoints[m].x, polarPoints[m].y, polarPoints[m].z }; + edge.push_back(a_pt); + } + } + //edge_link1 + for (int m = 1; m < contourPtSize; m++) + { + int idx = (startIdx - m + contourPtSize) % contourPtSize; + SVzNL3DPoint a_pt = { polarPoints[idx].x, polarPoints[idx].y, polarPoints[idx].z }; + double dist = sqrt(pow(a_pt.x - polarPoints[startIdx].x, 2) + pow(a_pt.y - polarPoints[startIdx].y, 2)); + if (dist > workpieceParam.lineLen) + break; + edge_link1.push_back(a_pt); + } + //edge_link2 + for (int m = 1; m < contourPtSize; m++) + { + int idx = (endIdx + m) % contourPtSize; + SVzNL3DPoint a_pt = { polarPoints[idx].x, polarPoints[idx].y, polarPoints[idx].z }; + double dist = sqrt(pow(a_pt.x - polarPoints[endIdx].x, 2) + pow(a_pt.y - polarPoints[endIdx].y, 2)); + if (dist > workpieceParam.lineLen) + break; + edge_link2.push_back(a_pt); + } + + region[rgnIdx].rgnIdx = rgnIdx; + region[rgnIdx].edge.insert(region[rgnIdx].edge.end(), edge.begin(), edge.end()); + region[rgnIdx].edgeLink_1.insert(region[rgnIdx].edgeLink_1.end(), edge_link1.begin(), edge_link1.end()); + region[rgnIdx].edgeLink_2.insert(region[rgnIdx].edgeLink_2.end(), edge_link2.begin(), edge_link2.end()); + } } - region[1].rgnIdx = 1; //Top - _getEdgeContour(&v_trees[vTree_T], region[1].edge, scanLines, true); - //ѰҶӦ - firstPt = region[1].edge[0]; - lastPt = region[1].edge.back(); - idx0 = _getPointClosestContour(h_trees, false, firstPt, scanLines, true); - if (idx0 < 0) +#if 0 //㷨ҪɨΪˮƽ-ֱ { - *errCode = SX_ERR_INVLD_CLOSES_PT; - return workpieceCorners; - } - _getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[1].edgeLink_1, scanLines, true, workpieceParam.lineLen); - idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true); - if (idx1 < 0) - { - *errCode = SX_ERR_INVLD_CLOSES_PT; - return workpieceCorners; - } - _getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[1].edgeLink_2, scanLines, true, workpieceParam.lineLen); - if ((region[1].edgeLink_1.size() < 5) || (region[1].edgeLink_2.size() < 5)) - { - *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; - return workpieceCorners; - } - region[2].rgnIdx = 2; //Right - _getEdgeContour(&h_trees[hTree_R], region[2].edge, scanLines, false); - //ѰҶӦ - firstPt = region[2].edge[0]; - lastPt = region[2].edge.back(); - idx0 = _getPointClosestContour(v_trees, true, firstPt, scanLines, false); - if (idx0 < 0) - { - *errCode = SX_ERR_INVLD_CLOSES_PT; - return workpieceCorners; - } - _getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[2].edgeLink_1, scanLines, false, workpieceParam.lineLen); - idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, false); - if (idx1 < 0) - { - *errCode = SX_ERR_INVLD_CLOSES_PT; - return workpieceCorners; - } - _getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[2].edgeLink_2, scanLines, false, workpieceParam.lineLen); - if ((region[2].edgeLink_1.size() < 5) || (region[2].edgeLink_2.size() < 5)) - { - *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; - return workpieceCorners; - } - region[3].rgnIdx = 3; //Bottom - _getEdgeContour(&v_trees[vTree_B], region[3].edge, scanLines, true); - //ѰҶӦ - firstPt = region[3].edge[0]; - lastPt = region[3].edge.back(); - idx0 = _getPointClosestContour(h_trees, false, firstPt, scanLines, true); - if (idx0 < 0) - { - *errCode = SX_ERR_INVLD_CLOSES_PT; - return workpieceCorners; - } - _getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[3].edgeLink_1, scanLines, false, workpieceParam.lineLen); - idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true); - if (idx1 < 0) - { - *errCode = SX_ERR_INVLD_CLOSES_PT; - return workpieceCorners; - } - _getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[3].edgeLink_2, scanLines, false, workpieceParam.lineLen); - if ((region[3].edgeLink_1.size() < 5) || (region[3].edgeLink_2.size() < 5)) - { - *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; - return workpieceCorners; + //ֱȡ + std::vector> jumpFeatures_v; + for (int line = 0; line < lineNum; line++) + { + if (line == 202) + int kkk = 1; + + std::vector& lineData = scanLines[line]; + if (linePtNum != (int)lineData.size()) + isGridData = false; + + //˲˳쳣 + sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam); + + std::vector line_features; + int dataSize = (int)lineData.size(); + sg_getLineCornerFeature_BQ( + &lineData[0], + dataSize, + line, + groundCalibPara.planeHeight, + cornerPara, //scaleͨȡbagH1/4 + line_features); + jumpFeatures_v.push_back(line_features); + } + + if (false == isGridData)//ݲʽ + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return workpieceCorners; + } + + //ˮƽɨ + std::vector> hLines; + hLines.resize(linePtNum); + for (int i = 0; i < linePtNum; i++) + hLines[i].resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + scanLines[line][j].nPointIdx = 0; //ԭʼݵ0תʹã + hLines[j][line] = scanLines[line][j]; + hLines[j][line].pt3D.x = scanLines[line][j].pt3D.y; + hLines[j][line].pt3D.y = scanLines[line][j].pt3D.x; + } + } + //ˮƽarcȡ + std::vector> jumpFeatures_h; + int lineNum_h = (int)hLines.size(); + for (int line = 0; line < lineNum_h; line++) + { + if (line == 416) + int kkk = 1; + std::vector& lineData = hLines[line]; + //˲˳쳣 + int ptNum = (int)lineData.size(); + sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam); + + std::vector line_features; + int dataSize = (int)lineData.size(); + sg_getLineCornerFeature_BQ( + &hLines[line][0], + dataSize, + line, + groundCalibPara.planeHeight, + cornerPara, //scaleͨȡbagH1/4 + line_features); + jumpFeatures_h.push_back(line_features); + } + + // + //ֱ߷ + std::vector v_trees; + for (int line = 0; line < lineNum; line++) + { + bool isLastLine = false; + if (line == lineNum - 1) + isLastLine = true; + std::vector& a_lineJumpFeature = jumpFeatures_v[line]; + if (a_lineJumpFeature.size() > 0) + int kkk = 1; + if (line == 202) + int kkk = 1; + sg_lineFeaturesGrowing( + line, + isLastLine, + a_lineJumpFeature, + v_trees, + growParam); + } + + //ˮƽɨ˶ + std::vector h_trees; + for (int line = 0; line < lineNum_h; line++) + { + if (line == 650) + int kkk = 1; + bool isLastLine = false; + if (line == lineNum_h - 1) + isLastLine = true; + std::vector& a_lineJumpFeature = jumpFeatures_h[line]; + sg_lineFeaturesGrowing( + line, + isLastLine, + a_lineJumpFeature, + h_trees, + growParam); + } + + //treeϢ + std::vector allTreesInfo; //߽ + SSG_treeInfo a_nullTree; + memset(&a_nullTree, 0, sizeof(SSG_treeInfo)); + allTreesInfo.push_back(a_nullTree); //ִ洢λtreeIdxͬλã + //ǣбע + int hvTreeIdx = 1; + for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++) + { + SSG_featureTree* a_vTree = &v_trees[i]; + sg_getTreeROI(a_vTree); + //¼TreeϢ + SSG_treeInfo a_treeInfo; + a_treeInfo.vTreeFlag = 1; + a_treeInfo.treeIdx = hvTreeIdx; + a_treeInfo.treeType = a_vTree->treeType; + a_treeInfo.sLineIdx = a_vTree->sLineIdx; + a_treeInfo.eLineIdx = a_vTree->eLineIdx; + a_treeInfo.roi = a_vTree->roi; + allTreesInfo.push_back(a_treeInfo); + + std::vector a_weld_contour; + //ԭʼϱǣͬʱMaskϱ + for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++) + { + SSG_basicFeature1D* a_feature = &a_vTree->treeNodes[j]; + if (scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.z > 1e-4)//Ŀ˺0 + { + int existEdgeId = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx >> 16; + if (existEdgeId == 0) + { + scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType; + scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx &= 0xffff; + scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx += hvTreeIdx << 16; + } + a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D); + } + } + hvTreeIdx++; + } + int hTreeStart = hvTreeIdx; + ////ע:ˮƽ + for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++) + { + SSG_featureTree* a_hTree = &h_trees[i]; + sg_getTreeROI(a_hTree); + //¼TreeϢ + SSG_treeInfo a_treeInfo; + a_treeInfo.vTreeFlag = 0; + a_treeInfo.treeIdx = hvTreeIdx; + a_treeInfo.treeType = a_hTree->treeType; + a_treeInfo.sLineIdx = a_hTree->sLineIdx; + a_treeInfo.eLineIdx = a_hTree->eLineIdx; + a_treeInfo.roi.left = a_hTree->roi.top; //ˮƽɨxyǽ + a_treeInfo.roi.right = a_hTree->roi.bottom; + a_treeInfo.roi.top = a_hTree->roi.left; + a_treeInfo.roi.bottom = a_hTree->roi.right; + allTreesInfo.push_back(a_treeInfo); + + std::vector a_weld_contour; + //ԭʼϱǣͬʱMaskϱ + for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++) + { + SSG_basicFeature1D* a_feature = &a_hTree->treeNodes[j]; + if (scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.z > 1e-4)//Ŀ˺0 + { + int existEdgeId = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx >> 16; + if (existEdgeId == 0) + { + scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4; + scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx &= 0xffff; + scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += hvTreeIdx << 16; + } + a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D); + } + } + hvTreeIdx++; + } + int hvTreeSize = hvTreeIdx; + + if (v_trees.size() < 2) + { + *errCode = SX_ERR_INVLD_VTREE_NUM; + return workpieceCorners; + } + if (h_trees.size() < 2) + { + *errCode = SX_ERR_INVLD_HTREE_NUM; + return workpieceCorners; + } + + //ѰvTreeϺ + int vTree_T = 0; + int vTree_B = 0; + for (int i = 1, i_max = (int)v_trees.size(); i < i_max; i++) + { + if (v_trees[i].roi.top < v_trees[vTree_T].roi.top) + vTree_T = i; + if (v_trees[i].roi.bottom > v_trees[vTree_B].roi.bottom) + vTree_B = i; + } + + //ѰhTree + int hTree_L = 0; + int hTree_R = 0; + for (int i = 1, i_max = (int)h_trees.size(); i < i_max; i++) + { + //ˮƽɨxyǽģҶӦROItopBottom + if (h_trees[i].roi.top < h_trees[hTree_L].roi.top) + hTree_L = i; + if (h_trees[i].roi.bottom > h_trees[hTree_R].roi.bottom) + hTree_R = i; + } + + region[0].rgnIdx = 0; //Left + _getEdgeContour(&h_trees[hTree_L], region[0].edge, scanLines, false); + //ѰҶӦ + SVzNL3DPoint firstPt = region[0].edge[0]; + SVzNL3DPoint lastPt = region[0].edge.back(); + int idx0 = _getPointClosestContour(v_trees, true, firstPt, scanLines, true); + if (idx0 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } + _getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[0].edgeLink_1, scanLines, true, workpieceParam.lineLen); + int idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, true); + if (idx1 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } + _getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[0].edgeLink_2, scanLines, true, workpieceParam.lineLen); + if ((region[0].edgeLink_1.size() < 5) || (region[0].edgeLink_2.size() < 5)) + { + *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; + return workpieceCorners; + } + + region[1].rgnIdx = 1; //Top + _getEdgeContour(&v_trees[vTree_T], region[1].edge, scanLines, true); + //ѰҶӦ + firstPt = region[1].edge[0]; + lastPt = region[1].edge.back(); + idx0 = _getPointClosestContour(h_trees, false, firstPt, scanLines, true); + if (idx0 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } + _getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[1].edgeLink_1, scanLines, true, workpieceParam.lineLen); + idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true); + if (idx1 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } + _getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[1].edgeLink_2, scanLines, true, workpieceParam.lineLen); + if ((region[1].edgeLink_1.size() < 5) || (region[1].edgeLink_2.size() < 5)) + { + *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; + return workpieceCorners; + } + region[2].rgnIdx = 2; //Right + _getEdgeContour(&h_trees[hTree_R], region[2].edge, scanLines, false); + //ѰҶӦ + firstPt = region[2].edge[0]; + lastPt = region[2].edge.back(); + idx0 = _getPointClosestContour(v_trees, true, firstPt, scanLines, false); + if (idx0 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } + _getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[2].edgeLink_1, scanLines, false, workpieceParam.lineLen); + idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, false); + if (idx1 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } + _getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[2].edgeLink_2, scanLines, false, workpieceParam.lineLen); + if ((region[2].edgeLink_1.size() < 5) || (region[2].edgeLink_2.size() < 5)) + { + *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; + return workpieceCorners; + } + region[3].rgnIdx = 3; //Bottom + _getEdgeContour(&v_trees[vTree_B], region[3].edge, scanLines, true); + //ѰҶӦ + firstPt = region[3].edge[0]; + lastPt = region[3].edge.back(); + idx0 = _getPointClosestContour(h_trees, false, firstPt, scanLines, true); + if (idx0 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } + _getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[3].edgeLink_1, scanLines, false, workpieceParam.lineLen); + idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true); + if (idx1 < 0) + { + *errCode = SX_ERR_INVLD_CLOSES_PT; + return workpieceCorners; + } + _getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[3].edgeLink_2, scanLines, false, workpieceParam.lineLen); + if ((region[3].edgeLink_1.size() < 5) || (region[3].edgeLink_2.size() < 5)) + { + *errCode = SX_ERR_INVLD_EDGE_LINK_NUM; + return workpieceCorners; + } } +#endif for (int i = 0; i < 4; i++) { @@ -618,7 +1080,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_link2_kx, edge_link2_bx); region[i].edge_link2_ends[0] = { foot_0.y, foot_0.x, groundCalibPara.planeHeight }; region[i].edge_link2_ends[1] = { foot_1.y, foot_1.x, groundCalibPara.planeHeight }; - + //㽻 end_0 = region[i].edge[0]; end_1 = region[i].edge.back(); @@ -644,6 +1106,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( } } } + diff --git a/sourceCode/BQ_workpieceCornerExtraction_Export.h b/sourceCode/BQ_workpieceCornerExtraction_Export.h index b61d217..7e501d2 100644 --- a/sourceCode/BQ_workpieceCornerExtraction_Export.h +++ b/sourceCode/BQ_workpieceCornerExtraction_Export.h @@ -40,6 +40,9 @@ typedef struct SVzNL3DPoint edge_link2_ends[2]; }SSX_debugInfo; +//汾 +SG_APISHARED_EXPORT const char* wd_BQWorkpieceCornerVersion(void); + //һƽƽ //пһƽͲοƽƽ棬ߵƽеƽ //תΪƽƽ淨ΪֱIJ diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 45dd066..61efce0 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -22,7 +22,10 @@ SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier_changeOriginData( SVzNL3DPosition* lineData, int dataSize, SSG_outlierFilterParam filterParam); - +//˳Ⱥ㣺z޷, vecotr +SG_APISHARED_EXPORT void wd_vectorDataRemoveOutlier_overwrite( + std::vector& a_line, + SSG_outlierFilterParam filterParam); //˳Ⱥ㣺޷Ϊεж SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier_ptDistMethod( SVzNL3DPosition* lineData, @@ -30,6 +33,7 @@ SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier_ptDistMethod( SSG_outlierFilterParam filterParam, std::vector& filerData, std::vector& noisePts); + //ƽ SG_APISHARED_EXPORT void sg_lineDataSmoothing( std::vector& input, @@ -68,6 +72,24 @@ SG_APISHARED_EXPORT void sg_getLineCornerFeature_BQ( const SSG_cornerParam cornerPara, std::vector& line_features); +/// +/// ȡϵ䡢zֵVLͣ⣨PSM) +/// seg˵㣺z +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +SG_APISHARED_EXPORT void wd_getLineCornerFeature_PSM( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + const double groundZ, + const SSG_cornerParam cornerPara, + SSG_lineFeature* line_features); + /// /// ȡϵJumping /// nPointIdx¶Feature @@ -133,6 +155,13 @@ SG_APISHARED_EXPORT void sg_getFeatureGrowingTrees( std::vector& trees, SSG_treeGrowParam growParam); +//feature:ȽtypeֻжλǷ +SG_APISHARED_EXPORT void wd_getFeatureGrowingTrees_noTypeMatch( + std::vector& lineFeatures, + std::vector& feature_trees, + std::vector& ending_trees, + SSG_treeGrowParam growParam); + SG_APISHARED_EXPORT void sg_lineFeaturesGrowing( int lineIdx, bool isLastLine, @@ -281,6 +310,10 @@ SG_APISHARED_EXPORT SVzNL3DRangeD sg_getScanDataROI_vector( std::vector< std::vector>& scanLines ); +//ƵROIscale: vecotrʽ +SG_APISHARED_EXPORT SWD_pointCloudPara wd_getPointCloudPara( + std::vector< std::vector>& scanLines); + //XYƽֱ SG_APISHARED_EXPORT void lineFitting( std::vector< SVzNL3DPoint>& inliers, @@ -365,4 +398,19 @@ SG_APISHARED_EXPORT void sg_pointClustering( std::vector< SVzNL3DPosition>& pts, double clusterDist, std::vector>& objClusters //result -); \ No newline at end of file +); + +//դݽXYƽϵͶӰĿհ׵вֵ +SG_APISHARED_EXPORT void pointClout2DProjection( + std::vector< std::vector>& gridScanData, + SVzNLRangeD x_range, + SVzNLRangeD y_range, + double scale, + int edgeSkip, + double inerPolateDistTh, //ֵֵڴֵIJֵ + cv::Mat& projectionData,//ͶӰݣʼΪһֵ1e+6 + cv::Mat& backIndexing //ڻ3D +); + +//ˮ㷨 +SG_APISHARED_EXPORT void watershed(SWD_waterShedImage& img); \ No newline at end of file diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index 3ec07fd..be84d9e 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -4,7 +4,7 @@ #include #include -#define PI 3.141592654 +#define PI 3.14159265358979323846 // ŷǽṹ壨λȣ typedef struct{ @@ -199,6 +199,22 @@ typedef struct double bagH; // }SSG_bagParam; +typedef struct +{ + double length; // + double width; // + double height; // +}SWD_sizeParam; + +typedef struct +{ + SVzNLRangeD xRange; //< XΧ + SVzNLRangeD yRange; //< YΧ + SVzNLRangeD zRange; //< ZΧ + double scale_x; + double scale_y; +} SWD_pointCloudPara; + typedef struct { ESG_poseSortingMode sortMode; @@ -267,6 +283,15 @@ typedef struct int value; }SSG_RUN; +typedef struct +{ + int start; + int len; + int value; + bool start_zRising; //z־ + bool end_zRising;//յz־ +}SSG_RUN_EX; + typedef struct { double mean; @@ -414,3 +439,11 @@ typedef struct double forward_z; double backward_z; }SSG_pntDirAngle; + +// ͼݽṹ +typedef struct { + int width; + int height; + std::vector> gray; // Ҷͼ + std::vector> markers; // ͼ-1ʾˮ룬0ʾδǣ>0ʾ +}SWD_waterShedImage; diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index ead9928..0bd8a8b 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -5,6 +5,7 @@ #include #include +//ɨROI SVzNL3DRangeD sg_getScanDataROI( SVzNL3DLaserLine* laser3DPoints, int lineNum) @@ -65,6 +66,7 @@ SVzNL3DRangeD sg_getScanDataROI( return roi; } +//ɨROI: vecotrʽ SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector>& scanLines) { SVzNL3DRangeD roi; @@ -125,6 +127,93 @@ SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector return roi; } +//ƵROIscale: vecotrʽ +SWD_pointCloudPara wd_getPointCloudPara(std::vector< std::vector>& scanLines) +{ + SWD_pointCloudPara para; + para.xRange = { 0, -1 }; + para.yRange = { 0, -1 }; + para.zRange = { 0, -1 }; + para.scale_x = -1; //ʼֵ + para.scale_y = -1; + int lineNum = (int)scanLines.size(); + double x_scale = 0; + int x_scale_cnt = 0; + double y_scale = 0; + double y_scale_cnt = 0; + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = (int)scanLines[line].size(); + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanLines[line][i]; + if (pt3D->pt3D.z < 1e-4) + continue; + + if (i > 0) + { + if (scanLines[line][i - 1].pt3D.z > 1e-4) + { + y_scale += abs(pt3D->pt3D.y - scanLines[line][i - 1].pt3D.y); + y_scale_cnt++; + } + } + if (line > 0) + { + if (scanLines[line - 1][i].pt3D.z > 1e-4) + { + x_scale += abs(pt3D->pt3D.x - scanLines[line-1][i].pt3D.x); + x_scale_cnt++; + } + } + + if (para.xRange.max < para.xRange.min) + { + para.xRange.min = pt3D->pt3D.x; + para.xRange.max = pt3D->pt3D.x; + } + else + { + if (para.xRange.min > pt3D->pt3D.x) + para.xRange.min = pt3D->pt3D.x; + if (para.xRange.max < pt3D->pt3D.x) + para.xRange.max = pt3D->pt3D.x; + } + //y + if (para.yRange.max < para.yRange.min) + { + para.yRange.min = pt3D->pt3D.y; + para.yRange.max = pt3D->pt3D.y; + } + else + { + if (para.yRange.min > pt3D->pt3D.y) + para.yRange.min = pt3D->pt3D.y; + if (para.yRange.max < pt3D->pt3D.y) + para.yRange.max = pt3D->pt3D.y; + } + //z + if (para.zRange.max < para.zRange.min) + { + para.zRange.min = pt3D->pt3D.z; + para.zRange.max = pt3D->pt3D.z; + } + else + { + if (para.zRange.min > pt3D->pt3D.z) + para.zRange.min = pt3D->pt3D.z; + if (para.zRange.max < pt3D->pt3D.z) + para.zRange.max = pt3D->pt3D.z; + } + } + } + if (x_scale_cnt > 0) + para.scale_x = x_scale / (double)x_scale_cnt; + if (y_scale_cnt > 0) + para.scale_y = y_scale / (double)y_scale_cnt; + return para; +} + void lineFitting(std::vector< SVzNL3DPoint>& inliers, double* _k, double* _b) { //Сֱ߲ @@ -1979,4 +2068,98 @@ void lineDataRT_RGBD(SVzNLXYZRGBDLaserLine* a_line, const double* camPoseR, doub a_line->p3DPoint[i] = a_pt; } return; -} \ No newline at end of file +} + +//դݽXYƽϵͶӰĿհ׵вֵ +void pointClout2DProjection( + std::vector< std::vector>& gridScanData, + SVzNLRangeD x_range, + SVzNLRangeD y_range, + double scale, + int edgeSkip, + double inerPolateDistTh, //ֵֵڴֵIJֵ + cv::Mat& projectionData,//ͶӰݣʼΪһֵ1e+6 + cv::Mat& backIndexing //ڻ3D +) +{ + int lineNum = (int)gridScanData.size(); + if (lineNum == 0) + return; + int nPointCnt = (int)gridScanData[0].size(); + for (int line = 0; line < lineNum; line++) + { + int pre_x = -1, pre_y = -1; + SVzNL3DPosition* prePt = NULL; + for (int i = 0; i < nPointCnt; i++) + { + SVzNL3DPosition* pt3D = &gridScanData[line][i]; + if (pt3D->pt3D.z < 1e-4) + continue; + double x = pt3D->pt3D.x; + double y = pt3D->pt3D.y; + int px = (int)(x - x_range.min)/scale + edgeSkip; + int py = (int)(y - y_range.min)/scale + edgeSkip; + + cv::Vec2i v2i_exist = backIndexing.at(py, px); +#if 0 + if ((v2i_exist[0] > 0) || (v2i_exist[1] > 0)) //ظͶӰͬһϣֻһЧ + { + pt3D->pt3D.z = 0; //invalidate + } + else +#endif + { + cv::Vec2i v2i = { line, i }; + backIndexing.at(py, px) = v2i; + projectionData.at(py, px) = 1e+6; + //ֱֵ + if (prePt) + { + //룬һ򲻲ֵ + double dist = sqrt(pow(pt3D->pt3D.x - prePt->pt3D.x, 2) + + pow(pt3D->pt3D.y - prePt->pt3D.y, 2) + + pow(pt3D->pt3D.z - prePt->pt3D.z, 2)); + if (dist < inerPolateDistTh) + { + std::vector interPts; + drawLine( + pre_x, + pre_y, + px, + py, + interPts); + for (int m = 0, m_max = (int)interPts.size(); m < m_max; m++) + projectionData.at(interPts[m].y, interPts[m].x) = 1e+6; + } + } + prePt = pt3D; + pre_x = px; + pre_y = py; + } + } + } + //ˮƽֵ + int pixWin = (int)(inerPolateDistTh / scale); + for (int y = 0; y < projectionData.rows; y++) + { + int pre_x = -1; + for (int x = 0; x < projectionData.cols; x++) + { + double value = projectionData.at(y, x); + if (value > 1e-4) + { + if (pre_x >= 0) + { + //ֵ + int x_diff = x - pre_x; + if ((x_diff > 1) && (x_diff < pixWin)) + { + for (int m = pre_x + 1; m < x; m++) + projectionData.at(y, m) = 1e+6; + } + } + pre_x = x; + } + } + } +} diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index a35b02a..afcf3bf 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -11,4 +11,7 @@ #define SX_ERR_INVLD_VTREE_NUM -2001 #define SX_ERR_INVLD_HTREE_NUM -2002 #define SX_ERR_INVLD_EDGE_LINK_NUM -2003 -#define SX_ERR_INVLD_CLOSES_PT -2004 \ No newline at end of file +#define SX_ERR_INVLD_CLOSES_PT -2004 +#define SX_ERR_ZERO_CONTOUR_PT -2005 +#define SX_ERR_INVLID_RPEAK_NUM -2006 +#define SX_ERR_INVLID_RPEAK_PAIR -2007 \ No newline at end of file diff --git a/sourceCode/SG_featureGrow.cpp b/sourceCode/SG_featureGrow.cpp index 465dbfd..71f9b5c 100644 --- a/sourceCode/SG_featureGrow.cpp +++ b/sourceCode/SG_featureGrow.cpp @@ -82,6 +82,37 @@ bool _featureGrowing(SSG_basicFeature1D& a_feature, const int lineIdx, std::vect return false; } +//featuretreesѰҺʵûкʵ㣬 false +//ûʹȫƥ䡣һfeatureһƥϣƥɡûʹƥ䡣 +bool _featureGrowing_noTypeMatch(SSG_basicFeature1D& a_feature, const int lineIdx, std::vector& trees, SSG_treeGrowParam growParam) +{ + for (int i = 0, i_max = (int)trees.size(); i < i_max; i++) + { + SSG_featureTree& a_tree = trees[i]; + if (TREE_STATE_DEAD == a_tree.treeState) + continue; + // + SSG_basicFeature1D last_node = a_tree.treeNodes.back(); + if (last_node.jumpPos2D.x == a_feature.jumpPos2D.x) //xΪlineIdxͬһɨϵIJ + continue; + + //ж + double y_diff = abs(a_feature.jumpPos.y - last_node.jumpPos.y); + double z_diff = abs(a_feature.jumpPos.z - last_node.jumpPos.z); + int line_diff = abs(a_feature.jumpPos2D.x - last_node.jumpPos2D.x); + double x_diff = abs(a_feature.jumpPos.x - last_node.jumpPos.x); + if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) && + ((line_diff < growParam.maxLineSkipNum) || (x_diff < growParam.maxSkipDistance))) + { + a_tree.eLineIdx = lineIdx; + a_tree.treeNodes.push_back(a_feature); + a_tree.tree_value += a_feature.featureValue; + return true; + } + } + return false; +} + int _getMatchedTree_angleCheck( SSG_basicFeature1D& a_feature, const int lineIdx, @@ -481,6 +512,95 @@ void sg_getFeatureGrowingTrees( } } +void wd_getFeatureGrowingTrees_noTypeMatch( + std::vector& lineFeatures, + std::vector& feature_trees, + std::vector& ending_trees, + SSG_treeGrowParam growParam) +{ + for (int i = 0, i_max = (int)lineFeatures.size(); i < i_max; i++) + { + SSG_lineFeature& a_line = lineFeatures[i]; + if (a_line.features.size() > 0) + { + for (int j = 0, j_max = (int)a_line.features.size(); j < j_max; j++) + { + SSG_basicFeature1D& a_feature = a_line.features[j]; + if (a_feature.jumpPos2D.x == 207) + int kkk = 1; + bool isMatched = _featureGrowing_noTypeMatch(a_feature, a_line.lineIdx, feature_trees, growParam); + if (false == isMatched) + { + //µ + SSG_featureTree a_newTree; + a_newTree.treeNodes.push_back(a_feature); + a_newTree.treeState = TREE_STATE_ALIVE; + a_newTree.treeType = a_feature.featureType; + a_newTree.sLineIdx = a_line.lineIdx; + a_newTree.eLineIdx = a_line.lineIdx; + feature_trees.push_back(a_newTree); + } + } + } + if (a_line.endings.size() > 0) + { + for (int j = 0, j_max = (int)a_line.endings.size(); j < j_max; j++) + { + SSG_basicFeature1D& a_feature = a_line.endings[j]; + if (a_feature.jumpPos2D.x == 207) + int kkk = 1; + bool isMatched = _featureGrowing_noTypeMatch(a_feature, a_line.lineIdx, ending_trees, growParam); + if (false == isMatched) + { + //µ + SSG_featureTree a_newTree; + a_newTree.treeNodes.push_back(a_feature); + a_newTree.treeState = TREE_STATE_ALIVE; + a_newTree.treeType = a_feature.featureType; + a_newTree.sLineIdx = a_line.lineIdx; + a_newTree.eLineIdx = a_line.lineIdx; + ending_trees.push_back(a_newTree); + } + } + } + //ֹͣ,١ + //ڵΪ1Ƴ + int lineIdx = a_line.lineIdx; + int m_max = (int)feature_trees.size(); + for (int m = m_max - 1; m >= 0; m--) //ӺǰɾӰѭ + { + if (TREE_STATE_ALIVE == feature_trees[m].treeState) + { + int line_diff = abs(lineIdx - feature_trees[m].treeNodes.back().jumpPos2D.x); + if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) || + (i == i_max - 1)) + { + feature_trees[m].treeState = TREE_STATE_DEAD; + bool isValid = _invalidateVSlopeTrees(feature_trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen); + if (false == isValid) + feature_trees.erase(feature_trees.begin() + m); + } + } + } + m_max = (int)ending_trees.size(); + for (int m = m_max - 1; m >= 0; m--) //ӺǰɾӰѭ + { + if (TREE_STATE_ALIVE == ending_trees[m].treeState) + { + int line_diff = abs(lineIdx - ending_trees[m].treeNodes.back().jumpPos2D.x); + if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) || + (i == i_max - 1)) + { + ending_trees[m].treeState = TREE_STATE_DEAD; + bool isValid = _invalidateVSlopeTrees(ending_trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen); + if (false == isValid) + ending_trees.erase(ending_trees.begin() + m); + } + } + } + } +} + void sg_lineFeaturesGrowing( int lineIdx, bool isLastLine, @@ -811,7 +931,6 @@ if (edge1_pts[i].x == 622) #endif } - void sg_getPeakGrowingTrees( std::vector>& peakFeatures, std::vector& trees, diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index 60bfa39..ad58a88 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -261,6 +261,62 @@ void sg_lineDataRemoveOutlier_changeOriginData( return; } +//˳Ⱥ㣺z޷, vecotr +void wd_vectorDataRemoveOutlier_overwrite( + std::vector& a_line, + SSG_outlierFilterParam filterParam) +{ + int dataSize = (int)a_line.size(); + std::vector< SSG_RUN> continueRuns; + SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx + double pre_z = 0; + for (int i = 0; i < dataSize; i++) + { + if (i == 370) + int kkk = 1; + if (a_line[i].pt3D.z > 1e-4) + { + if (a_run.len < 0) + { + a_run.start = i; + a_run.len = 1; + a_run.value = i; + } + else + { + double z_diff = abs(a_line[i].pt3D.z - pre_z); + if (z_diff < filterParam.continuityTh) + { + a_run.len++; + a_run.value = i; + } + else + { + continueRuns.push_back(a_run); + a_run.start = i; + a_run.len = 1; + a_run.value = i; + } + } + pre_z = a_line[i].pt3D.z; + } + } + if (a_run.len > 0) + continueRuns.push_back(a_run); + + for (int i = 0, i_max = (int)continueRuns.size(); i < i_max; i++) + { + if (continueRuns[i].len < filterParam.outlierTh) // + { + for (int j = continueRuns[i].start; j <= continueRuns[i].value; j++) + { + a_line[j].pt3D.z = 0; + } + } + } + return; +} + //˳Ⱥ㣺뷽 void sg_lineDataRemoveOutlier_ptDistMethod(SVzNL3DPosition* lineData, int dataSize, SSG_outlierFilterParam filterParam, std::vector& filerData, std::vector& noisePts) { @@ -1134,6 +1190,7 @@ void sg_getLineLVFeature(SVzNL3DPosition* lineData, int dataSize, int lineIdx, c /// /// ȡϵĹյ +/// seg˵㣺z /// nPointIdx¶Feature /// 㷨̣ /// 1ǰǺͺ @@ -1445,6 +1502,7 @@ void sg_getLineCornerFeature( /// /// ȡϵĹյˮƽ˵ΪCorner +/// seg˵㣺zy /// Segcornerȡ /// nPointIdx¶Feature /// 㷨̣ @@ -1787,11 +1845,351 @@ void sg_getLineCornerFeature_BQ( return; } +/// +/// ȡϵ䡢zֵVLͣ⣨PSM) +/// seg˵㣺z +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +void wd_getLineCornerFeature_PSM( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + const double groundZ, //ƽZgrondZĵΪĿ߽ + const SSG_cornerParam cornerPara, + SSG_lineFeature* line_features) +{ + line_features->lineIdx = lineIdx; + if (lineIdx == 538) + int kkk = 1; + + //groundZнض + for (int i = 0; i < dataSize; i++) + { + if (lineData[i].pt3D.z > groundZ) + lineData[i].pt3D.z = 0; + } + + //ȥ + std::vector< SVzNL3DPosition> vldPts; + std::vector< int> vldPtSegIdx; + std::vector segs; + std::vector backIndexing; + backIndexing.resize(dataSize); + + int runIdx = 1; + SSG_RUN_EX a_run = { 0, -1, 0, false, false }; //startIdx, len, lastIdx + double pre_z = 0; + double pre_y = 0; + for (int i = 0; i < dataSize; i++) + { + if (i == 370) + int kkk = 1; + if (lineData[i].pt3D.z > 1e-4) + { + if (a_run.len < 0) + { + a_run.start_zRising = true; + a_run.start = i; + a_run.len = 1; + a_run.end_zRising = true; + a_run.value = i; + } + else + { + double z_diff = abs(lineData[i].pt3D.z - pre_z); + if (z_diff < cornerPara.minEndingGap_z) + { + a_run.len = i - a_run.start + 1; + a_run.value = i; + } + else + { + bool next_zRising; + if (pre_z > lineData[i].pt3D.z) + { + a_run.end_zRising = true; + next_zRising = false; + } + else + { + a_run.end_zRising = false; + next_zRising = true; + } + a_run.value = runIdx; + runIdx++; + segs.push_back(a_run); + + a_run.start = i; + a_run.start_zRising = next_zRising; + a_run.len = 1; + a_run.value = i; + a_run.end_zRising = true; + } + } + int bIdx = (int)vldPts.size(); + backIndexing[i] = bIdx; + vldPts.push_back(lineData[i]); + vldPtSegIdx.push_back(runIdx); + + pre_z = lineData[i].pt3D.z; + } + } + if (a_run.len > 0) + segs.push_back(a_run); + + //ñ־ + for (int i = 0, i_max = (int)segs.size(); i < i_max; i++) + { + int idx1 = segs[i].start; + int idx2 = segs[i].start + segs[i].len - 1; + lineData[idx1].nPointIdx |= 0x100000; + lineData[idx2].nPointIdx |= 0x200000; + } + + //ǰǺͺ + std::vector< SSG_pntDirAngle> corners; + corners.resize(vldPts.size()); + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) + { + if ((lineIdx == 399) && (i == 419)) + int kkk = 1; + //ǰѰ + int pre_i = -1; + for (int j = i - 1; j >= 0; j--) + { + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if (dist >= cornerPara.scale) + { + pre_i = j; + break; + } + } + //Ѱ + int post_i = -1; + for (int j = i + 1; j < i_max; j++) + { + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if (dist >= cornerPara.scale) + { + post_i = j; + break; + } + } + //ս + if ((pre_i < 0) || (post_i < 0)) + { + corners[i].pntIdx = -1; + corners[i].forwardAngle = 0; + corners[i].backwardAngle = 0; + corners[i].corner = 0; + corners[i].forwardDiffZ = 0; + corners[i].backwardDiffZ = 0; + } + else + { + double tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y); + double tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y); + double forwardAngle = atan(tanValue_post) * 180.0 / PI; + double backwardAngle = atan(tanValue_pre) * 180.0 / PI; + corners[i].pntIdx = i; + corners[i].forwardAngle = forwardAngle; + corners[i].backwardAngle = backwardAngle; + corners[i].corner = -(forwardAngle - backwardAngle); //ͼϵϵy෴С- + corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z; + corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z; + } + } + + //սǼֵ + int _state = 0; + int pre_i = -1; + int sEdgePtIdx = -1; + int eEdgePtIdx = -1; + SSG_pntDirAngle* pre_data = NULL; + std::vector< SSG_pntDirAngle> cornerPeakP; + std::vector< SSG_pntDirAngle> cornerPeakM; + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) + { + if (i == 275) + int kkk = 1; + SSG_pntDirAngle* curr_data = &corners[i]; + if (curr_data->pntIdx < 0) + { + if (i == i_max - 1) //һ + { + if (1 == _state) // + { + cornerPeakP.push_back(corners[eEdgePtIdx]); + } + else if (2 == _state) //½ + { + cornerPeakM.push_back(corners[eEdgePtIdx]); + } + } + continue; + } + + if (NULL == pre_data) + { + sEdgePtIdx = i; + eEdgePtIdx = i; + pre_data = curr_data; + pre_i = i; + continue; + } + + eEdgePtIdx = i; + double cornerDiff = curr_data->corner - pre_data->corner; + switch (_state) + { + case 0: //̬ + if (cornerDiff < 0) //½ + { + _state = 2; + } + else if (cornerDiff > 0) // + { + _state = 1; + } + break; + case 1: // + if (cornerDiff < 0) //½ + { + cornerPeakP.push_back(*pre_data); + _state = 2; + } + break; + case 2: //½ + if (cornerDiff > 0) // + { + cornerPeakM.push_back(*pre_data); + _state = 1; + } + break; + default: + _state = 0; + break; + } + pre_data = curr_data; + pre_i = i; + } + //ע⣺һΪλ + + //Сֵ㣨嶥 + //ֵȽϣڳ߶ȴѰҾֲֵ + double square_distTh = 4 * cornerPara.scale * cornerPara.scale; //2cornerScale + for (int i = 0, i_max = (int)cornerPeakP.size(); i < i_max; i++) + { + if (cornerPeakP[i].corner < cornerPara.cornerTh) + continue; + + bool isPeak = true; + //ǰ + int cornerPtIdx = cornerPeakP[i].pntIdx; + for (int j = i - 1; j >= 0; j--) + { + int prePtIdx = cornerPeakP[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ; + if (dist > square_distTh) //߶ȴ + break; + + if (cornerPeakP[i].corner < cornerPeakP[j].corner) + { + isPeak = false; + break; + } + } + // + if (true == isPeak) + { + cornerPtIdx = cornerPeakP[i].pntIdx; + for (int j = i + 1; j < i_max; j++) + { + int postPtIdx = cornerPeakP[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2); + if (dist > square_distTh) //߶ȴ + break; + + if (cornerPeakP[i].corner < cornerPeakP[j].corner) + { + isPeak = false; + break; + } + } + } + if (true == isPeak) + { + SSG_basicFeature1D a_feature; + if ((cornerPeakP[i].backwardAngle > cornerPara.jumpCornerTh_1) && (cornerPeakP[i].forwardAngle > -cornerPara.jumpCornerTh_2)) + a_feature.featureType = LINE_FEATURE_L_JUMP_H2L; + else if ((cornerPeakP[i].forwardAngle < -cornerPara.jumpCornerTh_1) && (cornerPeakP[i].backwardAngle < cornerPara.jumpCornerTh_2)) + a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; + else + a_feature.featureType = LINE_FEATURE_CORNER_V; + + + a_feature.jumpPos = vldPts[cornerPtIdx].pt3D; + a_feature.jumpPos2D = { lineIdx, vldPts[cornerPtIdx].nPointIdx }; + line_features->features.push_back(a_feature); + } + } + + //ӿʼͽ߽ + //segǷҪϲϲ + for (int i = 0, i_max = (int)segs.size(); i < i_max - 1; i++) + { + SSG_RUN_EX* nxt_seg = &segs[i + 1]; + SSG_RUN_EX* curr_seg = &segs[i]; + + int idx_1 = curr_seg->start + curr_seg->len - 1; + int idx_2 = nxt_seg->start; + double y_diff = abs(lineData[idx_1].pt3D.y - lineData[idx_2].pt3D.y); + double z_diff = abs(lineData[idx_1].pt3D.z - lineData[idx_2].pt3D.z); + if ((y_diff < cornerPara.minEndingGap) && (z_diff < cornerPara.minEndingGap_z)) //ϲ + { + int idx_end = nxt_seg->start + nxt_seg->len - 1; + nxt_seg->start = curr_seg->start; + nxt_seg->len = idx_end - curr_seg->start + 1; + curr_seg->value = 0; + } + } + for (int i = 0, i_max = (int)segs.size(); i < i_max; i++) + { + if (0 == segs[i].value) //ϲ + continue; + + int idx_1 = segs[i].start; + int idx_2 = segs[i].start + segs[i].len - 1; + + SSG_basicFeature1D an_edge; + memset(&an_edge, 0, sizeof(SSG_basicFeature1D)); + an_edge.featureType = LINE_FEATURE_LINE_ENDING_0; + an_edge.jumpPos = lineData[idx_1].pt3D; + an_edge.jumpPos2D = { lineIdx, idx_1 }; + line_features->endings.push_back(an_edge); + //line_features.insert(line_features.begin(), an_edge); //ͷ + //β + an_edge.featureType = LINE_FEATURE_LINE_ENDING_1; + an_edge.jumpPos = lineData[idx_2].pt3D; + an_edge.jumpPos2D = { lineIdx, idx_2 }; + line_features->endings.push_back(an_edge); + } + return; +} + bool compareByIdx(const SSG_pntDirAngle& a, const SSG_pntDirAngle& b) { return a.pntIdx < b.pntIdx; } /// -/// ȡϵJumping +/// ȡϵJumping, ڴӺ +/// seg˵㣺z /// nPointIdx¶Feature /// 㷨̣ /// 1ǰǺͺ diff --git a/sourceCode/SG_regionGrow.cpp b/sourceCode/SG_regionGrow.cpp index 0f615fd..f6692ce 100644 --- a/sourceCode/SG_regionGrow.cpp +++ b/sourceCode/SG_regionGrow.cpp @@ -1359,4 +1359,3 @@ void sg_getLocalPeaks_distTransform(cv::Mat& input, std::vector& p } return; } -