diff --git a/hybridPosePositioning_test/hybridPosePositioning_test.cpp b/hybridPosePositioning_test/hybridPosePositioning_test.cpp index f572584..4b356f3 100644 --- a/hybridPosePositioning_test/hybridPosePositioning_test.cpp +++ b/hybridPosePositioning_test/hybridPosePositioning_test.cpp @@ -1223,7 +1223,8 @@ void _outputWorkpieceInfo(char* fileName, std::vector< WD_workpieceInfo>& workpi } //sprintf_s(dataStr, 250, "宸ヤ欢_%d: id=%d, type=%d", i + 1, workpiecePositions[i].id, workpiecePositions[i].workpieceType); sw << dataStr << std::endl; - + sprintf_s(dataStr, 50, " imgCenter: (%d, %d)", workpiecePositions[i].imgCenter.x, workpiecePositions[i].imgCenter.y); + sw << dataStr << std::endl; sprintf_s(dataStr, 50, " center: (%g, %g, %g)", workpiecePositions[i].center.x, workpiecePositions[i].center.y, workpiecePositions[i].center.z); sw << dataStr << std::endl; sprintf_s(dataStr, 50, " x_dir: (%g, %g, %g)", workpiecePositions[i].x_dir.x, workpiecePositions[i].x_dir.y, workpiecePositions[i].x_dir.z); diff --git a/sourceCode/hybridPosePositioning.cpp b/sourceCode/hybridPosePositioning.cpp index 4aca26b..2510e4b 100644 --- a/sourceCode/hybridPosePositioning.cpp +++ b/sourceCode/hybridPosePositioning.cpp @@ -23,7 +23,8 @@ //version 1.3.10 : 转子钢芯, 将分层系数由内部参数转为输入参数 //version 1.3.11 : 转子钢芯, 将边距系数由内部参数转为输入参数 //version 1.4.0 : 转子钢芯, 添加了隔板的识别和定位。 当定位隔板时,工件类型为9 -std::string m_strVersion = "HybridPositioning 1.4.0"; +//version 1.4.1 : 转子钢芯, 每个输出结果添加了2D图像坐标 +std::string m_strVersion = "HybridPositioning 1.4.1"; const char* wd_hybridPositioningVersion(void) { return m_strVersion.c_str(); @@ -1418,6 +1419,45 @@ void spacerBoardDetection( return; } +void _searchImgPosition(std::vector< std::vector>& scanLinesInput, const SVzNL3DPoint& pos_3D, SVzNL2DPoint& pos_2D) +{ + SVzNLPositionD bestPt; + memset(&bestPt, 0, sizeof(SVzNLPositionD)); + double minDist = -1; + for (int line = 0; line < (int)scanLinesInput.size(); line++) + { + for (int j = 0; j < (int)scanLinesInput[line].size(); j++) + { + if (scanLinesInput[line][j].pt3D.z < 1e-4) + continue; + + if (minDist < -1e-4) + { + bestPt = scanLinesInput[line][j]; + minDist = abs(scanLinesInput[line][j].pt3D.x - pos_3D.x) + + abs(scanLinesInput[line][j].pt3D.y - pos_3D.y) + + abs(scanLinesInput[line][j].pt3D.z - pos_3D.z); + } + else + { + double dist = abs(scanLinesInput[line][j].pt3D.x - pos_3D.x) + + abs(scanLinesInput[line][j].pt3D.y - pos_3D.y) + + abs(scanLinesInput[line][j].pt3D.z - pos_3D.z); + if (minDist > dist) + { + bestPt = scanLinesInput[line][j]; + minDist = dist; + } + } + } + } + if (minDist >= -1e-4) + pos_2D = { (int)bestPt.ptLeft2D.x, (int)bestPt.ptLeft2D.y }; + else + pos_2D = { 0, 0 }; + return; +} + void wd_HRM_RotorCorePositioning( std::vector< std::vector>& scanLinesInput, std::vector& objROIs, @@ -1774,6 +1814,7 @@ void wd_HRM_RotorCorePositioning( workpieceParam, outsideEdgePtIndice, centerPtIndice); + a_workpiece.imgCenter = { nCx, nCy }; a_workpiece.id = idx + 1; #ifdef _OUTPUT_DEBUG_DATA @@ -1806,6 +1847,7 @@ void wd_HRM_RotorCorePositioning( if (spacerInfo.center.z > 1e-4) //隔板 { WD_workpieceInfo a_workpiece; + memset(&a_workpiece, 0, sizeof(WD_workpieceInfo)); a_workpiece.center = spacerInfo.center; a_workpiece.id = 0; a_workpiece.workpieceType = 9; //隔板 @@ -1834,6 +1876,7 @@ void wd_HRM_RotorCorePositioning( { allWorkpiece.clear(); WD_workpieceInfo a_workpiece; + memset(&a_workpiece, 0, sizeof(WD_workpieceInfo)); a_workpiece.center = spacerInfo.center; a_workpiece.id = 0; a_workpiece.workpieceType = 9; //隔板 @@ -2138,6 +2181,11 @@ void wd_HRM_RotorCorePositioning( workpiecePositions[i].z_dir = rpt; rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix); workpiecePositions[i].x_dir = rpt; + + if (workpiecePositions[i].workpieceType == 9) + { + _searchImgPosition(scanLinesInput, workpiecePositions[i].center, workpiecePositions[i].imgCenter); + } } #ifdef _OUTPUT_DEBUG_DATA diff --git a/sourceCode/hybridPosePositioning_Export.h b/sourceCode/hybridPosePositioning_Export.h index 611e3d8..35688a9 100644 --- a/sourceCode/hybridPosePositioning_Export.h +++ b/sourceCode/hybridPosePositioning_Export.h @@ -16,6 +16,7 @@ typedef struct { int id; //序号从1开始 int workpieceType; //0:中央; 1-上边; 2-下边; 3-左边; 4-右边; 5-左上角; 6-右上角; 7-左下角; 8-右下角; 9-隔板 + SVzNL2DPoint imgCenter; //图像坐标 SVzNL3DPoint center; SVzNL3DPoint z_dir; //z方向向量(归一化) SVzNL3DPoint y_dir; //y方向向量(归一化)