From bf8b449e48397035582b76a68e39572be1ee7ffb Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Tue, 25 Aug 2026 23:41:55 +0800 Subject: [PATCH] =?UTF-8?q?hybridPosePositioning=20version=201.3.9=20:=20?= =?UTF-8?q?=E8=BD=AC=E5=AD=90=E9=92=A2=E8=8A=AF=EF=BC=8C=20=E6=94=B9?= =?UTF-8?q?=E8=BF=9B=E4=BA=86=E6=96=99=E7=AD=90=E6=A3=80=E6=B5=8B=E6=96=B9?= =?UTF-8?q?=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../hybridPosePositioning_test.cpp | 131 ++++++++++-- sourceCode/hybridPosePositioning.cpp | 192 +++++++++++++++--- sourceCode/hybridPosePositioning_Export.h | 3 +- 3 files changed, 286 insertions(+), 40 deletions(-) diff --git a/hybridPosePositioning_test/hybridPosePositioning_test.cpp b/hybridPosePositioning_test/hybridPosePositioning_test.cpp index 2104ad3..171c90b 100644 --- a/hybridPosePositioning_test/hybridPosePositioning_test.cpp +++ b/hybridPosePositioning_test/hybridPosePositioning_test.cpp @@ -674,6 +674,7 @@ void _outputRGBDResult_XYZ_RGBD_workpieceInfo( char* fileName, std::vector>& scanLines, std::vector& poseInfo, + WD_HRM_BinInfo* binInfo, double dirLen) { int objNumber = 0; @@ -793,6 +794,55 @@ void _outputRGBDResult_XYZ_RGBD_workpieceInfo( sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; lineIdx++; } + + if (binInfo) + { + rgb = { 0, 0, 250 }; + //输出外接矩形 + SVzNL3DPoint dirPt1 = binInfo->minRectVertex[0]; + SVzNL3DPoint dirPt2 = binInfo->minRectVertex[1]; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << dirPt1.x << "," << dirPt1.y << "," << dirPt1.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << dirPt2.x << "," << dirPt2.y << "," << dirPt2.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + dirPt1 = binInfo->minRectVertex[1]; + dirPt2 = binInfo->minRectVertex[2]; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << dirPt1.x << "," << dirPt1.y << "," << dirPt1.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << dirPt2.x << "," << dirPt2.y << "," << dirPt2.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + dirPt1 = binInfo->minRectVertex[2]; + dirPt2 = binInfo->minRectVertex[3]; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << dirPt1.x << "," << dirPt1.y << "," << dirPt1.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << dirPt2.x << "," << dirPt2.y << "," << dirPt2.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + dirPt1 = binInfo->minRectVertex[3]; + dirPt2 = binInfo->minRectVertex[0]; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << dirPt1.x << "," << dirPt1.y << "," << dirPt1.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << dirPt2.x << "," << dirPt2.y << "," << dirPt2.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + } } sw.close(); @@ -1651,6 +1701,7 @@ void _XOYprojection_workpieceInfo( cv::Mat& img, std::vector>& dataLines, std::vector& workpieceInfo, + std::vector& binRectVertex, const double scale, const int sideWidth, const SVzNLRangeD x_range, @@ -1719,6 +1770,24 @@ void _XOYprojection_workpieceInfo( cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), 2); } } + if (binRectVertex.size() == 4) + { + for (int i = 0; i < 4; i++) + { + int idx_0 = i; + int idx_1 = (i + 1) % 4; + double x1 = binRectVertex[idx_0].x; + double y1 = binRectVertex[idx_0].y; + int px1 = (int)((x1 - x_range.min) / scale + sideWidth); + int py1 = (int)((y1 - y_range.min) / scale + sideWidth); + double x2 = binRectVertex[idx_1].x; + double y2 = binRectVertex[idx_1].y; + int px2 = (int)((x2 - x_range.min) / scale + sideWidth); + int py2 = (int)((y2 - y_range.min) / scale + sideWidth); + rgb = { 0, 0, 255 }; //y轴用绿色 + cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 2); + } + } } void _genXOYProjectionImage_workpieceInfo( @@ -1726,6 +1795,7 @@ void _genXOYProjectionImage_workpieceInfo( std::vector>& scanLines, const double scale, std::vector& workpieceInfo, + WD_HRM_BinInfo* binInfo, const double rpy[3], const double dirLen) { //旋转视角显示 @@ -1749,7 +1819,40 @@ void _genXOYProjectionImage_workpieceInfo( SVzNLRangeD rx_range, ry_range; _rotateCloudPts(scanLines, matrix3d, rotateLines, rx_range, ry_range); - //统计X和Y的范围 + std::vector binRectVertex;//最小外接矩形顶点 + if (binInfo) + { + binRectVertex.resize(4); + for (int i = 0; i < 4; i++) + { + binRectVertex[i] = _ptRotate(binInfo->minRectVertex[i], matrix3d); + if (rx_range.max < rx_range.min) + { + rx_range.min = binRectVertex[i].x; + rx_range.max = binRectVertex[i].x; + } + else + { + if (rx_range.min > binRectVertex[i].x) + rx_range.min = binRectVertex[i].x; + if (rx_range.max < binRectVertex[i].x) + rx_range.max = binRectVertex[i].x; + } + if (ry_range.max < ry_range.min) + { + ry_range.min = binRectVertex[i].y; + ry_range.max = binRectVertex[i].y; + } + else + { + if (ry_range.min > binRectVertex[i].y) + ry_range.min = binRectVertex[i].y; + if (ry_range.max < binRectVertex[i].y) + ry_range.max = binRectVertex[i].y; + } + } + } + int x_cols = (int)((rx_range.max - rx_range.min) / scale) + 1; if (x_cols % 2 == 1) x_cols += 1; @@ -1763,7 +1866,7 @@ void _genXOYProjectionImage_workpieceInfo( cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); //计算投影比例 - _XOYprojection_workpieceInfo(img, rotateLines, rotatedWorkpiece, scale, sideWidth, rx_range, ry_range, dirLen); + _XOYprojection_workpieceInfo(img, rotateLines, rotatedWorkpiece, binRectVertex, scale, sideWidth, rx_range, ry_range, dirLen); cv::imwrite(fileName, img); return; } @@ -2145,17 +2248,18 @@ void HaiRuiMa_groundCalib_test(void) } //拓普发工件孔定位（工件定位) -#define HRM_RotorCore_TEST_GROUP 2 +#define HRM_RotorCore_TEST_GROUP 3 void HaiRuiMa_rotorCorePositioning_test(void) { const char* dataPath[HRM_RotorCore_TEST_GROUP] = { "F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/数据2D3D/", //0 "F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/WorkpieceRotor/", //1 + "F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/现场数据/" }; SVzNLRange fileIdx[HRM_RotorCore_TEST_GROUP] = { - {1,10}, {3,70} + {1,10}, {3,70},{1,1} }; const char* ver = wd_hybridPositioningVersion(); @@ -2183,7 +2287,7 @@ void HaiRuiMa_rotorCorePositioning_test(void) for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { - //fidx = 26; + //fidx = 50; char _scan_file[256]; sprintf_s(_scan_file, "%s%d_cloud.txt", dataPath[grp], fidx); std::vector> scanLines; @@ -2199,6 +2303,7 @@ void HaiRuiMa_rotorCorePositioning_test(void) #endif WD_HRM_BinInfo binInfo; +#if 0 if (grp == 1) { std::vector< std::vector> scanData; @@ -2217,13 +2322,13 @@ void HaiRuiMa_rotorCorePositioning_test(void) double zSliceRange = 50.0; //取料筐最上边沿的Z切片范围 binInfo = wd_HRM_getBinSize(scanData, groundCalibPara, binHeight, zSliceRange, &rCode); } - else + else if(grp == 0) { //读取料筐参数 sprintf_s(calibFile, "%sbin_info.txt", dataPath[grp]); binInfo = _readBinInfo(calibFile); } - +#endif sprintf_s(_scan_file, "%s%d_result.txt", dataPath[grp], fidx); std::vector objROIs; if( (grp == 0) && (fidx <= 9)) @@ -2235,6 +2340,7 @@ void HaiRuiMa_rotorCorePositioning_test(void) workpieceParam.radius = 45.0; workpieceParam.height = 20.0; + double groundRemoveOffset = 200; //料筐地面去除的Z偏置，实际地面去除高度为 groundCalibPara.planeHeight - groundRemoveOffset long t1 = (long)GetTickCount64();//统计时间 int errCode = 0; @@ -2243,10 +2349,11 @@ void HaiRuiMa_rotorCorePositioning_test(void) wd_HRM_RotorCorePositioning( scanLines, objROIs, - binInfo, groundCalibPara, workpieceParam, + groundRemoveOffset, workpiecePositions, + binInfo, #ifdef _OUTPUT_DEBUG_DATA debugScanLines, #endif @@ -2277,14 +2384,14 @@ void HaiRuiMa_rotorCorePositioning_test(void) workpiecePositions[i].x_dir = rpt; } sprintf_s(_scan_file, "%sresult/result_%d.txt", dataPath[grp], fidx); - _outputRGBDResult_XYZ_RGBD_workpieceInfo(_scan_file, debugScanLines, workpiecePositions, 50); + _outputRGBDResult_XYZ_RGBD_workpieceInfo(_scan_file, debugScanLines, workpiecePositions, &binInfo, 50); #if 1 sprintf_s(calibFile, "%sresult/result_%d_projection.png", dataPath[grp], fidx); double rpy[3] = { 0, 0, 0 }; //{ 0,-45, 0 }; // double angleDrawLen = 15; double displayScale = 0.3; cv::String imgName(calibFile); - _genXOYProjectionImage_workpieceInfo(imgName, debugScanLines, displayScale, workpiecePositions, rpy, angleDrawLen); + _genXOYProjectionImage_workpieceInfo(imgName, debugScanLines, displayScale, workpiecePositions, &binInfo, rpy, angleDrawLen); #endif printf("done\n"); } @@ -2487,7 +2594,7 @@ void HaiRuiMa_workpieceSizeCompute_test(void) a_pose.value = workpieceInfo.workpieceRadius; a_pose.z_dir = { 0, 0, 1.0 }; poseInfo.push_back(a_pose); - _outputRGBDResult_XYZ_RGBD_workpieceInfo(_scan_file, scanLines, poseInfo, 500); + _outputRGBDResult_XYZ_RGBD_workpieceInfo(_scan_file, scanLines, poseInfo, NULL, 500); sprintf_s(_scan_file, "%sresult/%d_workpiece_info.txt", dataPath[grp], fidx); _outputWorkpieceInfo(_scan_file, workpieceInfo); @@ -2496,7 +2603,7 @@ void HaiRuiMa_workpieceSizeCompute_test(void) double angleDrawLen = 500; double displayScale = 1.0; cv::String imgName(calibFile); - _genXOYProjectionImage_workpieceInfo(imgName, scanLines, displayScale, poseInfo, rpy, angleDrawLen); + _genXOYProjectionImage_workpieceInfo(imgName, scanLines, displayScale, poseInfo, NULL, rpy, angleDrawLen); printf("done\n"); } } diff --git a/sourceCode/hybridPosePositioning.cpp b/sourceCode/hybridPosePositioning.cpp index b8f5cd7..2630860 100644 --- a/sourceCode/hybridPosePositioning.cpp +++ b/sourceCode/hybridPosePositioning.cpp @@ -19,7 +19,8 @@ //version 1.3.6 : 1.3.5汾ĻϸֳתӸоλ㷨еbug //version 1.3.7 : 1.3.6汾ĻID 2Dһ //version 1.3.8 : դʽ飬 ԵĿΪĿϿߵľֱ1.1 -std::string m_strVersion = "HybridPositioning 1.3.8"; +//version 1.3.9 : תӸо ĽϿⷽ +std::string m_strVersion = "HybridPositioning 1.3.9"; const char* wd_hybridPositioningVersion(void) { return m_strVersion.c_str(); @@ -937,13 +938,160 @@ void _getYTopLine( return; } +double _getListMeanZ(std::vector< SVzNL3DPosition>& listData, SVzNLRangeD& zRange) +{ + if (listData.size() == 0) + return 0; + double meanZ = 0; + zRange.max = -1; + zRange.min = 0; + for (int i = 0; i < (int)listData.size(); i++) + { + meanZ += listData[i].pt3D.z; + if (zRange.max < 0) + { + zRange.max = listData[i].pt3D.z; + zRange.min = listData[i].pt3D.z; + } + else + { + zRange.max = zRange.max < listData[i].pt3D.z ? listData[i].pt3D.z : zRange.max; + zRange.min = zRange.min > listData[i].pt3D.z ? listData[i].pt3D.z : zRange.min; + } + } + meanZ = meanZ / (double)listData.size(); + return meanZ; +} + +//ϿԵĵΪ׼й滮 +WD_HRM_BinInfo wd_HRM_getBinSize_2( + std::vector< std::vector>& scanLines, + const SVzNLRangeD zSliceRange) +{ + WD_HRM_BinInfo resultPose; + memset(&resultPose, 0, sizeof(WD_HRM_BinInfo)); + + int lineNum = (int)scanLines.size(); + int linePtNum = (int)scanLines[0].size(); + + //Z + std::vector zBinSliceData; + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + scanLines[line][j].nPointIdx = 0; + if ((scanLines[line][j].pt3D.z > zSliceRange.min) && (scanLines[line][j].pt3D.z < zSliceRange.max)) + { + SVzNL3DPosition a_pt; + a_pt.nPointIdx = (line << 16) | (j & 0xffff); + a_pt.pt3D = scanLines[line][j].pt3D; + zBinSliceData.push_back(a_pt); + scanLines[line][j].nPointIdx = 1; //ע + } + } + } + + // + int clusterCheckWin = 5; + double clusterDist = 10.0; + int distType = 1; //0 - 2d distance; 1- 3d distance + std::vector> objClusters; + wd_pointClustering_speedUp(zBinSliceData, lineNum, linePtNum, clusterCheckWin, clusterDist, distType, objClusters); + if(objClusters.size() == 0) + return resultPose; + + int bestIdx = 0; + for (int i = 1; i < (int)objClusters.size(); i++) + { + if (objClusters[bestIdx].size() < objClusters[i].size()) + bestIdx = i; + } + + std::vector& zSliceData = objClusters[bestIdx]; + SVzNLRangeD dataZRange; + double zSliceZ = _getListMeanZ(zSliceData, dataZRange); + // + // СӾ + std::vector points; + for (int i = 0; i < (int)zSliceData.size(); i++) + { + cv::Point2f a_pt = cv::Point2f(zSliceData[i].pt3D.x, zSliceData[i].pt3D.y); + points.push_back(a_pt); + } + if (points.size() == 0) + { + return resultPose; + } + + cv::RotatedRect rect = minAreaRect(points); + cv::Point2f vertices[4]; + rect.points(vertices); + double width = rect.size.width; //ͶӰĿ͸ + double height = rect.size.height; + if (width < height) + { + double tmp = height; + height = width; + width = tmp; + } + //̬vertices[0]ת + double dist_v0v3 = sqrt(pow(vertices[0].x - vertices[3].x, 2) + pow(vertices[0].y - vertices[3].y, 2)); + double width_diff = abs(dist_v0v3 - width); + double pose_yaw; + if (CV_VERSION == "3.2.0") + { + if (width_diff < 10.0)//width + { + pose_yaw = -rect.angle; + } + else //ȷ + { + pose_yaw = -rect.angle - 90; + if (pose_yaw < -90) + pose_yaw = 180 + pose_yaw; + } + } + else //if (CV_VERSION == "4.8.0") + { + if (width_diff < 10.0) //width + { + pose_yaw = -rect.angle; + } + else//ȷ + { + pose_yaw = -rect.angle + 90; + if (pose_yaw > 90) + pose_yaw = pose_yaw - 180; + } + } + //ϿϢ + double binZ = zSliceRange.min; + double sinTheta = sin(-PI * pose_yaw / 180); + double cosTheta = cos(-PI * pose_yaw / 180); + resultPose.center = { rect.center.x, rect.center.y, binZ }; + resultPose.bottomNormal = { 0.0, 0.0, 1.0 }; + resultPose.x_dir = { cosTheta , sinTheta , 0.0 }; + resultPose.y_dir = vec3_cross(resultPose.bottomNormal, resultPose.x_dir); //˳y_dir + resultPose.length = width; + resultPose.width = height; + resultPose.binTopZ = zSliceZ; + resultPose.minRectVertex[0] = { vertices[0].x, vertices[0].y, binZ }; + resultPose.minRectVertex[1] = { vertices[1].x, vertices[1].y, binZ }; + resultPose.minRectVertex[2] = { vertices[2].x, vertices[2].y, binZ }; + resultPose.minRectVertex[3] = { vertices[3].x, vertices[3].y, binZ }; + + return resultPose; +} + void wd_HRM_RotorCorePositioning( std::vector< std::vector>& scanLinesInput, std::vector& objROIs, - const WD_HRM_BinInfo binInfo, const SSG_planeCalibPara groundCalibPara, const SWD_Cylinder workpieceParam, //Բιıư뾶͸߶ + const double groundRemoveOffset, //ϿȥZƫãʵʵȥ߶Ϊ groundCalibPara.planeHeight - groundRemoveOffset std::vector< WD_workpieceInfo>& workpiecePositions, + WD_HRM_BinInfo& binInfo, //BinϢdebugȷϿϢȷ #ifdef _OUTPUT_DEBUG_DATA std::vector>& debugScanLines, #endif @@ -1025,6 +1173,14 @@ void wd_HRM_RotorCorePositioning( wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1); } + //ݸBinϢ + std::vector> dataCopy; + dataCopy.resize(scanLines.size()); + for (int i = 0; i < lineNum; i++) + { + dataCopy[i].insert(dataCopy[i].end(), scanLines[i].begin(), scanLines[i].end()); + } + //ˮƽɨ std::vector< std::vector> scanLines_h; scanLines_h.resize(linePtNum); @@ -1294,6 +1450,13 @@ void wd_HRM_RotorCorePositioning( double highest_z = allWorkpiece[0].center.z; for (int i = 1; i < (int)allWorkpiece.size(); i++) highest_z = highest_z > allWorkpiece[i].center.z ? allWorkpiece[i].center.z : highest_z; + + //ϿϢ + SVzNLRangeD zSliceRange; + zSliceRange.min = highest_z - workpieceParam.height; + zSliceRange.max = groundCalibPara.planeHeight - groundRemoveOffset; + binInfo = wd_HRM_getBinSize_2(dataCopy, zSliceRange); + //z std::vector< WD_workpieceInfo> zSortWorkpiece; double topLayerTh = highest_z + workpieceParam.height * 0.75; @@ -1747,31 +1910,6 @@ SSG_ROIRectD _getListROI(std::vector< SVzNL3DPosition>& listData) return roi; } -double _getListMeanZ(std::vector< SVzNL3DPosition>& listData, SVzNLRangeD& zRange) -{ - if (listData.size() == 0) - return 0; - double meanZ = 0; - zRange.max = -1; - zRange.min = 0; - for (int i = 0; i < (int)listData.size(); i++) - { - meanZ += listData[i].pt3D.z; - if (zRange.max < 0) - { - zRange.max = listData[i].pt3D.z; - zRange.min = listData[i].pt3D.z; - } - else - { - zRange.max = zRange.max < listData[i].pt3D.z ? listData[i].pt3D.z : zRange.max; - zRange.min = zRange.min > listData[i].pt3D.z ? listData[i].pt3D.z : zRange.min; - } - } - meanZ = meanZ / (double)listData.size(); - return meanZ; -} - //ʱתʱ > 0 ˳ʱתʱ < 0 cv::Point2f _rotate2D(cv::Point2f pt, double sinTheta, double cosTheta) { diff --git a/sourceCode/hybridPosePositioning_Export.h b/sourceCode/hybridPosePositioning_Export.h index ac5c5d2..9e8d6aa 100644 --- a/sourceCode/hybridPosePositioning_Export.h +++ b/sourceCode/hybridPosePositioning_Export.h @@ -61,10 +61,11 @@ SG_APISHARED_EXPORT void wd_lineDataR( SG_APISHARED_EXPORT void wd_HRM_RotorCorePositioning( std::vector< std::vector>& scanLinesInput, std::vector& objROIs, - const WD_HRM_BinInfo binInfo, const SSG_planeCalibPara groundCalibPara, const SWD_Cylinder workpieceParam, //Բιıư뾶͸߶ + const double groundRemoveOffset, //ϿȥZƫãʵʵȥ߶Ϊ groundCalibPara.planeHeight - groundRemoveOffset std::vector< WD_workpieceInfo>& workpiecePositions, + WD_HRM_BinInfo& binInfo, //BinϢdebugȷϿϢȷ #ifdef _OUTPUT_DEBUG_DATA std::vector>& debugScanLines, #endif