From 6674c32adca26f4d454454e39be6acfaab244c34 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Sun, 30 Aug 2026 09:43:29 +0800 Subject: [PATCH] =?UTF-8?q?rodAndBarDetection=20version=201.6.0=20:=20?= =?UTF-8?q?=E7=9F=A9=E6=A3=AE=E6=A3=92=E6=9D=90=E6=8A=93=E5=8F=96=E7=AE=97?= =?UTF-8?q?=E6=B3=95=E6=94=B9=E8=BF=9B=EF=BC=9A=E6=B7=BB=E5=8A=A0=E4=BA=86?= =?UTF-8?q?=E6=96=99=E7=AD=90=E4=BF=A1=E6=81=AF=E6=8F=90=E5=8F=96=E5=92=8C?= =?UTF-8?q?=E5=AF=B9=E8=BE=B9=E7=BC=98=E5=B7=A5=E4=BB=B6=E7=9A=84=E5=88=A4?= =?UTF-8?q?=E6=96=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../rodAndBarDetection_test.cpp | 198 +++++++++- sourceCode/SG_baseDataType.h | 13 + sourceCode/rodAndBarDetection.cpp | 364 ++++++++++++++++-- sourceCode/rodAndBarDetection_Export.h | 6 + 4 files changed, 538 insertions(+), 43 deletions(-) diff --git a/rodAndBarDetection_test/rodAndBarDetection_test.cpp b/rodAndBarDetection_test/rodAndBarDetection_test.cpp index 8e6620f..50b045b 100644 --- a/rodAndBarDetection_test/rodAndBarDetection_test.cpp +++ b/rodAndBarDetection_test/rodAndBarDetection_test.cpp @@ -680,7 +680,8 @@ void _outputRGBDScan_RGBD_centerPose( void _outputRGBDScan_RGBD_rodInfo( char* fileName, std::vector>& scanLines, - std::vector& rodInfo + std::vector& rodInfo, + SWD_BinInfo* binInfo ) { int lineNum = (int)scanLines.size(); @@ -735,11 +736,16 @@ void _outputRGBDScan_RGBD_rodInfo( rgb = { 180, 0, 0 }; size = 2; } - else + else if(pt3D->nPointIdx < 3) { rgb = objColor[pt3D->nPointIdx % 8]; size = 2; } + else //料筐 + { + rgb = { 0, 200,200 }; + size = 2; + } } else //if (pt3D->nPointIdx == 0) { @@ -761,11 +767,13 @@ void _outputRGBDScan_RGBD_rodInfo( if (objNum < (int)scanLines[0].size()) lastLineNum = (int)scanLines[0].size(); sw << "Line_" << lineIdx << "_0_" << lastLineNum << std::endl; - size = 12; + size = 25; for (int i = 0; i < objNum; i++) { - if(i == 0) - rgb = { 250, 255, 0 }; + if (i == 0) + rgb = { 250, 250, 0 }; + else if (rodInfo[i].workpieceType > 0) //边上目标 + rgb = { 0, 250, 250 }; else rgb = { 250, 0, 0 }; float x = (float)rodInfo[i].center.x; @@ -816,6 +824,55 @@ void _outputRGBDScan_RGBD_rodInfo( lineIdx++; } 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(); } @@ -1323,14 +1380,38 @@ void _convertToGridData_XYZRGB(std::vector>& sca return; } +void rotationMatrixToEulerZYX1(const double R[9], double rpy[3]) { + // 计算俯仰角（pitch）θ + double pitch = asin(-R[6]); // asin返回弧度 + double yaw, roll; + // 检查万向节锁（cosθ接近0） + const double epsilon = 1e-6; + if (abs(cos(pitch)) > epsilon) { + // 无万向节锁，正常计算yaw和roll + yaw = atan2(R[3], R[0]); + roll = atan2(R[7], R[8]); + } + else { + // 万向节锁，约定roll=0，计算yaw + roll = 0.0; + yaw = atan2(-R[1], R[4]); + } + + // 将弧度转换为角度 + const double rad2deg = 180.0 / M_PI; + rpy[2] = yaw* rad2deg; + rpy[1] = pitch* rad2deg; + rpy[0] = roll* rad2deg; + return; +} void EulerRpyToRotation1(const double rpy[3], double matrix3d[9]) { - double cos0 = cos(rpy[0] * PI / 180); + double cos0 = cos(rpy[0] * PI / 180); double sin0 = sin(rpy[0] * PI / 180); double cos1 = cos(rpy[1] * PI / 180); double sin1 = sin(rpy[1] * PI / 180); double cos2 = cos(rpy[2] * PI / 180); double sin2 = sin(rpy[2] * PI / 180); - matrix3d[0] = cos2 * cos1; + matrix3d[0] = cos2 * cos1; matrix3d[1] = cos2 * sin1 * sin0 - sin2 * cos0; matrix3d[2] = cos2 * sin1 * cos0 + sin2 * sin0; matrix3d[3] = sin2 * cos1; @@ -1406,6 +1487,7 @@ void _XOYprojection_rodInfo( cv::Mat& img, std::vector>& dataLines, std::vector& rodInfo, + std::vector& binRectVertex, const double scale, const int sideWidth, const SVzNLRangeD x_range, @@ -1441,11 +1523,16 @@ void _XOYprojection_rodInfo( rgb = { 180, 0, 0 }; size = 2; } - else + else if(pt3D.nPointIdx < 3) { rgb = objColor[pt3D.nPointIdx % 8]; size = 2; } + else //料筐 + { + rgb = { 0, 200, 200 }; + size = 2; + } } else //if (pt3D->nPointIdx == 0) { @@ -1464,11 +1551,12 @@ void _XOYprojection_rodInfo( } if (rodInfo.size() > 0) { - for (int i = 0; i < (int)rodInfo.size(); i++) { if (i == 0) rgb = { 255, 255, 0 }; + else if (rodInfo[i].workpieceType > 0) //边上目标 + rgb = { 0, 250, 250 }; else rgb = { 255, 0, 0 }; size = int(10.0 / scale); @@ -1486,6 +1574,24 @@ void _XOYprojection_rodInfo( cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), size); } } + 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 }; + cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 2); + } + } } void _genXOYProjectionImage_rodInfo( @@ -1493,6 +1599,7 @@ void _genXOYProjectionImage_rodInfo( std::vector>& scanLines, const double scale, std::vector& rodInfo, + SWD_BinInfo* binInfo, const double rpy[3], const double dirLen) { //旋转视角显示 @@ -1503,6 +1610,8 @@ void _genXOYProjectionImage_rodInfo( for (int i = 0; i < (int)rodInfo.size(); i++) { SSX_rodPositionInfo rotateObj; + rotateObj.id = rodInfo[i].id; + rotateObj.workpieceType = rodInfo[i].workpieceType; rotateObj.center = _ptRotate(rodInfo[i].center, matrix3d); rotateObj.axialDir = _ptRotate(rodInfo[i].axialDir, matrix3d); rotateObj.normalDir = _ptRotate(rodInfo[i].normalDir, matrix3d); @@ -1515,6 +1624,40 @@ void _genXOYProjectionImage_rodInfo( SVzNLRangeD rx_range, ry_range; _rotateCloudPts(scanLines, matrix3d, rotateLines, rx_range, ry_range); + 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; + } + } + } + //统计X和Y的范围 int x_cols = (int)((rx_range.max - rx_range.min) / scale) + 1; if (x_cols % 2 == 1) @@ -1529,12 +1672,12 @@ void _genXOYProjectionImage_rodInfo( cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); //计算投影比例 - _XOYprojection_rodInfo(img, rotateLines, rotatedRod, scale, sideWidth, rx_range, ry_range, dirLen); + _XOYprojection_rodInfo(img, rotateLines, rotatedRod, binRectVertex, scale, sideWidth, rx_range, ry_range, dirLen); cv::imwrite(fileName, img); return; } -#define ROD_POSITION_TEST_GROUP 7 +#define ROD_POSITION_TEST_GROUP 8 #define TEST_CONVERT_TO_GRID 0 #define TEST_COMPUTE_CALIB_PARA 0 #define TEST_COMPUTE_ROD_POSITION 1 @@ -1641,17 +1784,18 @@ void rodPositionTest(void) "F:/ShangGu/项目/冠钦项目/矩森棒材抓取/异常点云/", //5 "F:/ShangGu/项目/冠钦项目/矩森棒材抓取/异常点云2/", //6 + "F:/ShangGu/项目/冠钦项目/矩森棒材抓取/异常点云3/", //7 }; SVzNLRange fileIdx[ROD_POSITION_TEST_GROUP] = { {1,8}, {1,3}, {1,31}, - {1,5},{1,11},{1,26},{1,41} + {1,5},{1,11},{1,26},{1,41}, {1, 23} }; const char* ver = wd_rodAndBarDetectionVersion(); printf("ver:%s\n", ver); - for (int grp = 0; grp <= 6; grp++) + for (int grp = 7; grp <= 7; grp++) { SSG_planeCalibPara poseCalibPara; //初始化成单位阵 @@ -1687,6 +1831,8 @@ void rodPositionTest(void) sprintf_s(_scan_file, "%s%d_LaserData_Ik256066.txt", dataPath[grp], fidx); else if (6 == grp) sprintf_s(_scan_file, "%s%d_LaserData_Ik256066.txt", dataPath[grp], fidx); + else if(7 == grp) + sprintf_s(_scan_file, "%s%d_cloud.txt", dataPath[grp], fidx); else sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx); @@ -1709,7 +1855,7 @@ void rodPositionTest(void) rodParam.diameter = 96.0; //圆棒直径 rodParam.len = 440; } - else if (grp == 6) + else if ( (grp == 6) ||(grp ==7)) { rodParam.diameter = 62.0; //圆棒直径 rodParam.len = 380; @@ -1740,9 +1886,18 @@ void rodPositionTest(void) growParam.minLTypeTreeLen = rodParam.len * 0.75; //mm, 螺杆长度 growParam.minVTypeTreeLen = rodParam.len * 0.75; //mm - bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道 + const double distToSideRatio = 1.1; //边距系数，边距门限=边距系数*工件直径；当工件距边的距离大于边距门限时，判断为边上的工件 + double groundRemoveOffset = 100; //料筐地面去除的Z偏置，实际地面去除高度为 groundCalibPara.planeHeight - groundRemoveOffset + double clusterNDist; //料筐聚类尺度， 当两点距离小于此尺度时， 被聚为一类。此参数和点云密度相关，用于滤除料筐周围的干扰。 + if ( grp == 5) + clusterNDist = 10.0;// 10.0; + else if (grp == 6) + clusterNDist = 15.0; + else if (grp == 7) + clusterNDist = 15.0; int errCode = 0; std::vector rodInfo; + SWD_BinInfo binInfo; sx_rodPositioning( scanLines, poseCalibPara, @@ -1750,14 +1905,18 @@ void rodPositionTest(void) filterParam, growParam, rodParam, + distToSideRatio, + groundRemoveOffset, + clusterNDist, rodInfo, + binInfo, &errCode); long t2 = (long)GetTickCount64(); printf("%s: %d(ms),errCode=%d\n", _scan_file, (int)(t2 - t1),errCode); //输出测试结果 sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx); - _outputRGBDScan_RGBD_rodInfo(_scan_file, scanLines, rodInfo); + _outputRGBDScan_RGBD_rodInfo(_scan_file, scanLines, rodInfo, &binInfo); sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx); _outputRodtInfo(_scan_file, rodInfo); #if 1 @@ -1766,7 +1925,12 @@ void rodPositionTest(void) double angleDrawLen = 15; double displayScale = 1.0; cv::String imgName(_scan_file); - _genXOYProjectionImage_rodInfo(imgName, scanLines, displayScale, rodInfo, rpy, angleDrawLen); + _genXOYProjectionImage_rodInfo(imgName, scanLines, displayScale, rodInfo, &binInfo, rpy, angleDrawLen); + //正投影 + sprintf_s(_scan_file, "%sresult/%d_result_projection_2.png", dataPath[grp], fidx); + rotationMatrixToEulerZYX1(poseCalibPara.planeCalib, rpy); + cv::String imgName2(_scan_file); + _genXOYProjectionImage_rodInfo(imgName2, scanLines, displayScale, rodInfo, &binInfo, rpy, angleDrawLen); #endif } } diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index 2e07ac1..251eb43 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -731,3 +731,16 @@ typedef struct SVzNL3DRangeD roi3D; SVzNLRect roi2D; }SWD_clustersInfo; + +//ϿϢ +typedef struct +{ + double length; + double width; + double binTopZ; //ϿߴZֵ + SVzNL3DPoint center; // + SVzNL3DPoint bottomNormal; //ķ + SVzNL3DPoint x_dir; //Xбǣ + SVzNL3DPoint y_dir; //Xбǣ + SVzNL3DPoint minRectVertex[4]; //СӾζ +}SWD_BinInfo; \ No newline at end of file diff --git a/sourceCode/rodAndBarDetection.cpp b/sourceCode/rodAndBarDetection.cpp index 57b505b..8f16833 100644 --- a/sourceCode/rodAndBarDetection.cpp +++ b/sourceCode/rodAndBarDetection.cpp @@ -31,7 +31,8 @@ //version 1.3.8 : µݸ˶λ㷨ʹPCAȷݸ //version 1.4.0 : ɭץȡ㷨Ľ1ıʶһ⣨2ǿ˶̶ȣǿ³ //version 1.5.0 : ɭץȡ㷨Ľǿ˶ǿƵĴ -std::string m_strVersion = "RodAndBarDetection_1.5.0"; +//version 1.6.0 : ɭץȡ㷨ĽϿϢȡͶԱԵж +std::string m_strVersion = "RodAndBarDetection_1.6.0"; const char* wd_rodAndBarDetectionVersion(void) { return m_strVersion.c_str(); @@ -243,6 +244,164 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9]) } +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; +} + +//ϿԵԾΪȡΪϿĵΪ׼й滮 +SWD_BinInfo wd_cluster_getBinSize( + std::vector< std::vector>& scanLines, + const SVzNLRangeD zSliceRange, + const double clusterNDist, + std::vector& binPoints) +{ + SWD_BinInfo resultPose; + memset(&resultPose, 0, sizeof(SWD_BinInfo)); + + int lineNum = (int)scanLines.size(); + int linePtNum = (int)scanLines[0].size(); + + //ȥ + SSG_outlierFilterParam filterParam; + filterParam.continuityTh = 5.0; //˳ڵzڴʱǷΪСoutlierLen Ϊ + filterParam.outlierTh = 3; + for (int line = 0; line < lineNum; line++) + wd_vectorDataRemoveOutlier_overwrite(scanLines[line], filterParam); + + //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 = 6; + double clusterDist = clusterNDist; + int distType = 0; //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; + } + + //Ͽƣdebug + binPoints.insert(binPoints.end(), zSliceData.begin(), zSliceData.end()); + + 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 rodArcFeatueDetection( std::vector< std::vector>& scanLines, const SSG_cornerParam cornerPara, @@ -1596,31 +1755,6 @@ void sx_hexHeadScrewMeasure_PCA( 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; -} - SSG_ROIRectD _getListROI(std::vector< SVzNL3DPosition>& listData) { if (listData.size() == 0) @@ -3389,7 +3523,11 @@ void sx_rodPositioning( const SSG_outlierFilterParam filterParam, const SSG_treeGrowParam growParam, const SSX_rodParam rodParam, + const double distToSideRatio, //߾ϵ߾=߾ϵ*ֱߵľڱ߾ʱжΪϵĹ + const double groundRemoveOffset, //ϿȥZƫãʵʵȥ߶Ϊ groundCalibPara.planeHeight - groundRemoveOffset + const double clusterNDist, //Ͽ߶ȣ Сڴ˳߶ʱ Ϊһࡣ˲͵ܶأ˳ϿΧĸš std::vector& rodInfo, + SWD_BinInfo& binInfo, //BinϢdebugȷϿϢȷ int* errCode) { *errCode = 0; @@ -3428,6 +3566,14 @@ void sx_rodPositioning( sx_rodPosition_lineDataR(scanLines[i], poseCalibPara.planeCalib, cuttingZ); } + //ݸ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> hLines_raw; hLines_raw.resize(linePtNum); @@ -3691,6 +3837,7 @@ void sx_rodPositioning( if (fittingPoints.size() < 3) continue; SSX_rodPositionInfo a_objRod; + memset(&a_objRod, 0, sizeof(SSX_rodPositionInfo)); _computeRodInfo(rodArcTrees_v[i], segEndings[m].nMin, segEndings[m].nMax, false, fittingPoints, a_objRod); rodInfo.push_back(a_objRod); } @@ -3760,6 +3907,7 @@ void sx_rodPositioning( continue; SSX_rodPositionInfo a_objRod; + memset(&a_objRod, 0, sizeof(SSX_rodPositionInfo)); _computeRodInfo(rodArcTrees_h[i], segEndings[m].nMin, segEndings[m].nMax, true, fittingPoints, a_objRod); //Ƿ봹ֱĿص bool isExist = checkObjEixst(a_objRod, rodInfo, rodParam); @@ -3772,9 +3920,162 @@ void sx_rodPositioning( //߶ std::sort(rodInfo.begin(), rodInfo.end(), _commpareByCenterZ); + //ϿϢ + double highest_z = rodInfo[0].center.z; + SVzNLRangeD zSliceRange; + zSliceRange.min = highest_z - rodParam.diameter * 0.5; + double groundTh = poseCalibPara.planeHeight - groundRemoveOffset; +#if 1 + zSliceRange.max = highest_z + rodParam.diameter * 4.5; + if (zSliceRange.max > groundTh) + zSliceRange.max = groundTh; +#else + zSliceRange.max = groundTh; +#endif + + std::vector binPoints; + binInfo = wd_cluster_getBinSize(dataCopy, zSliceRange, clusterNDist,binPoints); + //עϿݣdebug + for (int i = 0; i < (int)binPoints.size(); i++) + { + int lineIdx = binPoints[i].nPointIdx >> 16; + int ptIdx = binPoints[i].nPointIdx & 0xffff; + if (scanLines[lineIdx][ptIdx].nPointIdx == 0) + { + scanLines[lineIdx][ptIdx].nPointIdx = 3; + scanLines[lineIdx][ptIdx].pt3D = binPoints[i].pt3D; //ܵı˳ĵָ + } + } + + if (binInfo.binTopZ > 1e-4) + { + //㹤λ + std::vector linePoint; + linePoint.resize(4); + linePoint[0].pt1 = binInfo.minRectVertex[0]; + linePoint[0].pt2 = binInfo.minRectVertex[1]; + linePoint[1].pt1 = binInfo.minRectVertex[1]; + linePoint[1].pt2 = binInfo.minRectVertex[2]; + linePoint[2].pt1 = binInfo.minRectVertex[2]; + linePoint[2].pt2 = binInfo.minRectVertex[3]; + linePoint[3].pt1 = binInfo.minRectVertex[3]; + linePoint[3].pt2 = binInfo.minRectVertex[0]; + int lineTopIdx = -1; + for (int i = 0; i < (int)linePoint.size(); i++) + { + if ((linePoint[i].pt1.y < binInfo.center.y) && (linePoint[i].pt2.y < binInfo.center.y)) + { + lineTopIdx = i; + break; + } + } + if (lineTopIdx < 0) + { + *errCode = SX_ERR_BIN_EDGE_ERROR; + } + SWD_3DPointPair lineTop = linePoint[lineTopIdx]; + linePoint.erase(linePoint.begin() + lineTopIdx);//ȥѾʶedge + + int lineBtmIdx = -1; + for (int i = 0; i < (int)linePoint.size(); i++) + { + if ((linePoint[i].pt1.y > binInfo.center.y) && (linePoint[i].pt2.y > binInfo.center.y)) + { + lineBtmIdx = i; + break; + } + } + if (lineBtmIdx < 0) + { + *errCode = SX_ERR_BIN_EDGE_ERROR; + } + SWD_3DPointPair lineBtm = linePoint[lineBtmIdx]; + linePoint.erase(linePoint.begin() + lineBtmIdx); //ȥѾʶedge + + int lineLeftIdx = -1; + for (int i = 0; i < (int)linePoint.size(); i++) + { + if ((linePoint[i].pt1.x < binInfo.center.x) && (linePoint[i].pt2.x < binInfo.center.x)) + { + lineLeftIdx = i; + break; + } + } + if (lineLeftIdx < 0) + { + *errCode = SX_ERR_BIN_EDGE_ERROR; + } + SWD_3DPointPair lineLeft = linePoint[lineLeftIdx]; + linePoint.erase(linePoint.begin() + lineLeftIdx); //ȥѾʶedge + + int lineRightIdx = -1; + for (int i = 0; i < (int)linePoint.size(); i++) + { + if ((linePoint[i].pt1.x > binInfo.center.x) && (linePoint[i].pt2.x > binInfo.center.x)) + { + lineRightIdx = i; + break; + } + } + if (lineRightIdx < 0) + { + *errCode = SX_ERR_BIN_EDGE_ERROR; + } + if (*errCode == 0) + { + SWD_3DPointPair lineRight = linePoint[lineRightIdx]; + //㹤λãϡϡ¡ĸǣߣ벿 + //е۶ԲֵͬĹòͬץȡ + double distToSideTh = rodParam.diameter * distToSideRatio; + double lineTop_a, lineTop_b, lineTop_c; + compute2ptLine(lineTop.pt1, lineTop.pt2, &lineTop_a, &lineTop_b, &lineTop_c); + double lineBtm_a, lineBtm_b, lineBtm_c; + compute2ptLine(lineBtm.pt1, lineBtm.pt2, &lineBtm_a, &lineBtm_b, &lineBtm_c); + double lineLeft_a, lineLeft_b, lineLeft_c; + compute2ptLine(lineLeft.pt1, lineLeft.pt2, &lineLeft_a, &lineLeft_b, &lineLeft_c); + double lineRight_a, lineRight_b, lineRight_c; + compute2ptLine(lineRight.pt1, lineRight.pt2, &lineRight_a, &lineRight_b, &lineRight_c); + for (int i = 0; i < (int)rodInfo.size(); i++) //ϲĿз + { + SVzNL3DPoint& objCenter = rodInfo[i].center; + double distTop = computePtDistToLine(objCenter.x, objCenter.y, lineTop_a, lineTop_b, lineTop_c); + double distBtm = computePtDistToLine(objCenter.x, objCenter.y, lineBtm_a, lineBtm_b, lineBtm_c); + double distLeft = computePtDistToLine(objCenter.x, objCenter.y, lineLeft_a, lineLeft_b, lineLeft_c); + double distRight = computePtDistToLine(objCenter.x, objCenter.y, lineRight_a, lineRight_b, lineRight_c); + + if (distTop < distToSideTh) // + { + if (distLeft < distToSideTh) + rodInfo[i].workpieceType = 5; // + else if (distRight < distToSideTh) + rodInfo[i].workpieceType = 6; // + else + rodInfo[i].workpieceType = 1; // + } + else if (distBtm < distToSideTh) // + { + if (distLeft < distToSideTh) + rodInfo[i].workpieceType = 7; // + else if (distRight < distToSideTh) + rodInfo[i].workpieceType = 8; // + else + rodInfo[i].workpieceType = 2; // + } + else if (distLeft < distToSideTh) + rodInfo[i].workpieceType = 3; // + else if (distRight < distToSideTh) + rodInfo[i].workpieceType = 4; // + else + rodInfo[i].workpieceType = 0; // + } + } + + } + //ͶԭϵԱ۱궨ȷ for (int i = 0; i < lineNum; i++) sx_rodPosition_lineDataR(scanLines[i], poseCalibPara.invRMatrix, -1); + //Ͷԭϵ for (int i = 0; i < (int)rodInfo.size(); i++) { @@ -3791,6 +4092,17 @@ void sx_rodPositioning( rawObj = _translatePoint(a_rod.endPt, poseCalibPara.invRMatrix); a_rod.endPt = rawObj; } + + if (binInfo.binTopZ > 1e-4) + { + //binתԭϵ + binInfo.center = _translatePoint(binInfo.center, poseCalibPara.invRMatrix); + binInfo.bottomNormal = _translatePoint(binInfo.bottomNormal, poseCalibPara.invRMatrix); + binInfo.x_dir = _translatePoint(binInfo.x_dir, poseCalibPara.invRMatrix); + binInfo.y_dir = _translatePoint(binInfo.y_dir, poseCalibPara.invRMatrix); + for (int i = 0; i < 4; i++) + binInfo.minRectVertex[i] = _translatePoint(binInfo.minRectVertex[i], poseCalibPara.invRMatrix); + } return; } diff --git a/sourceCode/rodAndBarDetection_Export.h b/sourceCode/rodAndBarDetection_Export.h index bf2f05f..0987bdc 100644 --- a/sourceCode/rodAndBarDetection_Export.h +++ b/sourceCode/rodAndBarDetection_Export.h @@ -29,6 +29,8 @@ typedef struct typedef struct { + int id; //Ŵ1ʼ + int workpieceType; //0:; 1-ϱ; 2-±; 3-; 4-ұ; 5-Ͻ; 6-Ͻ; 7-½; 8-½; SVzNL3DPoint center; //ݸ˶˲ĵ SVzNL3DPoint axialDir; // SVzNL3DPoint normalDir; // @@ -123,7 +125,11 @@ SG_APISHARED_EXPORT void sx_rodPositioning( const SSG_outlierFilterParam filterParam, const SSG_treeGrowParam growParam, const SSX_rodParam rodParam, + const double distToSideRatio, //߾ϵ߾=߾ϵ*ֱߵľڱ߾ʱжΪϵĹ + const double groundRemoveOffset, //ϿȥZƫãʵʵȥ߶Ϊ groundCalibPara.planeHeight - groundRemoveOffset + const double clusterNDist, //Ͽ߶ȣ Сڴ˳߶ʱ Ϊһࡣ˲͵ܶأ˳ϿΧĸš std::vector& rodInfo, + SWD_BinInfo& binInfo, //BinϢdebugȷϿϢȷ int* errCode); //ԣֽṹֽ춨λ