diff --git a/sourceCode/hybridPosePositioning.cpp b/sourceCode/hybridPosePositioning.cpp index f050cb3..32ab8ba 100644 --- a/sourceCode/hybridPosePositioning.cpp +++ b/sourceCode/hybridPosePositioning.cpp @@ -13,7 +13,8 @@ //version 1.3.0 : 添加了海瑞马转子钢芯定位 //version 1.3.1 : 添加了海瑞马转子钢芯定位位置属性计算,区分出中央,边和角的位置,按照中心排序,先输出最中央的目标 //version 1.3.2 : 添加了海瑞马锥形工件的ID号,ID号为输入的ROI下标+1(从1开始) -std::string m_strVersion = "HybridPositioning 1.3.2"; +//version 1.3.3 : 改进了海瑞马锥形工件定位算法,优化的ROI重叠时的处理逻辑 +std::string m_strVersion = "HybridPositioning 1.3.3"; const char* wd_hybridPositioningVersion(void) { return m_strVersion.c_str(); @@ -1493,8 +1494,84 @@ void wd_HRM_TaperedWorkpiecePositioning( return; } + int lineNum = (int)scanLinesInput.size(); + int linePtNum = (int)scanLinesInput[0].size(); + int maxU = 0; //2D图像的最大Col + int maxV = 0; //2D图像的最大Row + for (int line = 0; line < lineNum; line++) + { + for (int ptIdx = 0; ptIdx < (int)scanLinesInput[line].size(); ptIdx++) + { + if (scanLinesInput[line][ptIdx].pt3D.z < 1e-4) + continue; + + maxU = maxU < scanLinesInput[line][ptIdx].ptLeft2D.x ? scanLinesInput[line][ptIdx].ptLeft2D.x : maxU; + maxV = maxV < scanLinesInput[line][ptIdx].ptLeft2D.y ? scanLinesInput[line][ptIdx].ptLeft2D.y : maxV; + } + } + + //生成图像与3d的对应表 + const int imgCols = maxU + 1; + const int imgRows = maxV + 1; + std::vector> mappingTable; //按图像大小建立。 + mappingTable.resize(imgCols); //与扫描线方向对应 + for (int i = 0; i < imgCols; i++) + mappingTable[i].resize(imgRows); + + std::vector> scanLines; + scanLines.resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + scanLines[line].resize(linePtNum); + for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++) + { + scanLinesInput[line][ptIdx].nPointIdx = ptIdx; + scanLines[line][ptIdx].pt3D = scanLinesInput[line][ptIdx].pt3D; + + if (scanLinesInput[line][ptIdx].pt3D.z < 1e-4) + continue; + + SWDScanPosition a_pos; + a_pos.lineIdx = line; + a_pos.ptIdx = ptIdx; + int u = scanLinesInput[line][ptIdx].ptLeft2D.x; + int v = scanLinesInput[line][ptIdx].ptLeft2D.y; + mappingTable[u][v].mappings.push_back(a_pos); + } + } + std::vector> rgnPoints; rgnPoints.resize(objROIs.size()); + for (int rgnId = 0; rgnId < (int)objROIs.size(); rgnId++) + { + WD_objArea2D& obj_roi = objROIs[rgnId]; + int L = (int)(obj_roi.roi.left); + int R = (int)(obj_roi.roi.right) + 1; + if (R > maxU) + R = maxU; + + int T = (int)(obj_roi.roi.top); + int B = (int)(obj_roi.roi.bottom) + 1; + if (B > maxV) + B = maxV; + + //统计ROI中的扫描线和PtIdx范围 + for (int x = L; x <= R; x++) + { + for (int y = T; y <= B; y++) + { + mapping2DTo3D& a_mapping = mappingTable[x][y]; + for (int m = 0; m < (int)a_mapping.mappings.size(); m++) + { + int lineIdx = a_mapping.mappings[m].lineIdx; + int ptIdx = a_mapping.mappings[m].ptIdx; + if (scanLines[lineIdx][ptIdx].pt3D.z > 1e-4) + rgnPoints[rgnId].push_back(scanLines[lineIdx][ptIdx]); + } + } + } + } +#if 0 for (int line = 0; line < (int)scanLinesInput.size(); line++) { for (int ptIdx = 0; ptIdx < (int)scanLinesInput[line].size(); ptIdx++) @@ -1514,7 +1591,7 @@ void wd_HRM_TaperedWorkpiecePositioning( } } } - +#endif //统计每个region的最高点 std::vector< SVzNL3DPosition> rgnPeaks; for (int i = 0; i < (int)rgnPoints.size(); i++)