diff --git a/sourceCode/BQ_workpieceCornerExtraction.cpp b/sourceCode/BQ_workpieceCornerExtraction.cpp index 6c35ddf..716374c 100644 --- a/sourceCode/BQ_workpieceCornerExtraction.cpp +++ b/sourceCode/BQ_workpieceCornerExtraction.cpp @@ -2122,6 +2122,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( featurePoints, lineNum, linePtNum, clusterCheckWin, //搜索窗口 clusterDist, + 0, //2D distance objClusters //result ); //取最大的聚类作为轮廓 diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 39fcf0d..7e8c414 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -4,6 +4,8 @@ #include #include +//向量叉乘 +SG_APISHARED_EXPORT SVzNL3DPoint vec3_cross(const SVzNL3DPoint& a, const SVzNL3DPoint& b); //滤除离群点:z跳变门限方法(大于门限视为不连续,根据连续段点数量判断噪声) SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier( diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 9cffbd5..09b146f 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -11,6 +11,15 @@ const double EPS = 1e-10; +SVzNL3DPoint vec3_cross(const SVzNL3DPoint& a, const SVzNL3DPoint& b) +{ + SVzNL3DPoint c; + c.x = a.y * b.z - a.z * b.y; + c.y = a.z * b.x - a.x * b.z; + c.z = a.x * b.y - a.y * b.x; + return c; +} + SVzNL3DRangeD sg_getScanDataROI( //计算扫描ROI SVzNL3DLaserLine* laser3DPoints, diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index 7f9a6db..fcc9992 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -44,3 +44,6 @@ //拓普发工件定位 #define SX_ERR_UNKNOWN_OBJECT -2501 +//配天工件定位 +#define SX_ERR_UNKNOWN_PLATE_DIR -2601 + diff --git a/sourceCode/rodAndBarDetection.cpp b/sourceCode/rodAndBarDetection.cpp index 189c493..094cd84 100644 --- a/sourceCode/rodAndBarDetection.cpp +++ b/sourceCode/rodAndBarDetection.cpp @@ -11,7 +11,8 @@ //version 1.2.0 : 配天螺杆测量增加了定位盘中心测量功能 //version 1.2.1 : 增加了定位盘中心完整姿态输出 //version 1.2.2 : 调整了定位盘中心姿态定义:法向(Z向):向右;Y:向下 -std::string m_strVersion = "1.2.2"; +//version 1.2.3 : 根据定向盘轮廓的直线段确定向右的向量 +std::string m_strVersion = "1.2.3"; const char* wd_rodAndBarDetectionVersion(void) { return m_strVersion.c_str(); @@ -481,6 +482,242 @@ SSG_ROIRectD _getListROI(std::vector< SVzNL3DPosition>& listData) return roi; } +bool _compareByAngle(const SWD_polarPt& a, const SWD_polarPt& b) { + return a.angle < b.angle; +} + +//计算封闭序列(首尾相连)在XOY平面内前向角和后向角以及拐角。 前向角:序号+; 后向角:序号- +void _computeClosedPntListDirCorners(std::vector& polarPoints, double scale, std::vector< SSG_dirCornerAngle>& dirCornerAngles) +{ + int pntSize = (int)polarPoints.size(); + //std::fill(dirCornerAngles.begin(), dirCornerAngles.end(), SSG_dirCornerAngle{ 0, 0, 0, 0, 0, 0 }); + dirCornerAngles.resize(pntSize); + for (int i = 0; i < pntSize; i++) + { + if (i == 68) + int kkk = 1; + memset(&dirCornerAngles[i], 0, sizeof(SSG_dirCornerAngle)); + SWD_polarPt& a_polarPt = polarPoints[i]; + //前向寻找 + int minus_i = -1; + for (int loop = i - 1; loop >= i - pntSize; loop--) + { + int j = loop >= 0 ? loop : (loop + pntSize); + double dist = sqrt(pow(polarPoints[i].x - polarPoints[j].x, 2) + + pow(polarPoints[i].y - polarPoints[j].y, 2)); + if (dist >= scale) + { + minus_i = j; + break; + } + } + //后向寻找 + int plus_i = -1; + for (int loop = i + 1; loop < i + pntSize; loop++) + { + int j = loop % pntSize; + double dist = sqrt(pow(polarPoints[i].x - polarPoints[j].x, 2) + + pow(polarPoints[i].y - polarPoints[j].y, 2)); + if (dist >= scale) + { + plus_i = j; + break; + } + } + //计算拐角 + if ((minus_i >= 0) && (plus_i >= 0)) + { + double backwardAngle = atan2(polarPoints[i].y - polarPoints[minus_i].y, polarPoints[i].x - polarPoints[minus_i].x) * 180.0 / PI; + double forwardAngle = atan2(polarPoints[plus_i].y - polarPoints[i].y, polarPoints[plus_i].x - polarPoints[i].x) * 180.0 / PI; + dirCornerAngles[i].forwardAngle = forwardAngle; + dirCornerAngles[i].backwardAngle = backwardAngle; + + double corner = forwardAngle - backwardAngle; + if (corner < -180) + corner += 360; + else if (corner > 180) + corner = corner - 360; + + dirCornerAngles[i].flag = 0; + //过滤掉由于极角和方向角相近产生的轮廓点位序错误导致的corner错误 + double polarAngle = dirCornerAngles[i].point.angle; + double angleDiff1 = abs(polarAngle - dirCornerAngles[i].forwardAngle); + if (angleDiff1 > 180) + angleDiff1 = 360 - angleDiff1; + double angleDiff2 = abs(polarAngle - dirCornerAngles[i].backwardAngle); + if (angleDiff2 > 180) + angleDiff2 = 360 - angleDiff2; + if ((angleDiff1 < 10) || (angleDiff2 < 10)) //过滤掉边与极线方向相近导致的波动 + { + if (dirCornerAngles[i].corner > 160) //乱序产生角度错误 + { + dirCornerAngles[i].flag = -1; + } + } + + dirCornerAngles[i].corner = corner; //图像坐标系与正常坐标系y方向相反,所以有“-”号 + dirCornerAngles[i].pntIdx = i; + dirCornerAngles[i].forward_pntIdx = plus_i; + dirCornerAngles[i].backward_pntIdx = minus_i; + dirCornerAngles[i].point = polarPoints[i]; + } + } +} + +//提取corner极值(较早实现函数可以使用此函数进行代码优化) +void _searchPlusCornerPeaks( + std::vector< SSG_dirCornerAngle>& corners, + double cutAngleTh, + std::vector< SSG_dirCornerAngle>& cornerPlusPeaks +) +{ + std::vector peakCorners; + int cornerSize = (int)corners.size(); + //搜索拐角极值 + int _state = 0; + int pre_i = -1; + int sEdgePtIdx = -1; + int eEdgePtIdx = -1; + SSG_dirCornerAngle* pre_data = NULL; + for (int i = 0, i_max = cornerSize; i < i_max; i++) + { + if (i == 451) + int kkk = 1; + SSG_dirCornerAngle* curr_data = &corners[i]; + if (curr_data->flag < 0) + 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) //下降 + { + if (pre_data->corner > cutAngleTh) //截角门限,滤除点波动导致的角度变化 + peakCorners.push_back(*pre_data); + _state = 2; + } + break; + case 2: //下降 + if (cornerDiff > 0) // 上升 + _state = 1; + break; + default: + _state = 0; + break; + } + pre_data = curr_data; + pre_i = i; + } + //注意:最后一个不处理,为基座位置 + + //极小值点(峰顶) + //极值比较,在尺度窗口下寻找局部极值点 + for (int i = 0, i_max = (int)peakCorners.size(); i < i_max; i++) + { + bool isPeak = true; + SSG_dirCornerAngle& a_dirAngle = peakCorners[i]; + int curr_dist1 = a_dirAngle.pntIdx - a_dirAngle.backward_pntIdx; + if (curr_dist1 < 0) + curr_dist1 += cornerSize; + int curr_dist2 = a_dirAngle.forward_pntIdx - a_dirAngle.pntIdx; + if (curr_dist2 < 0) + curr_dist2 += cornerSize; + + //minus方向寻找 + int minus_i = i; + while (1) + { + minus_i--; + if (minus_i < 0) + minus_i += i_max; + SSG_dirCornerAngle& minus_dirAngle = peakCorners[minus_i]; + int pntDist_0 = a_dirAngle.pntIdx - minus_dirAngle.pntIdx; + if (pntDist_0 < 0) + pntDist_0 += cornerSize; + if (pntDist_0 > curr_dist1) + break; + + if (a_dirAngle.corner < minus_dirAngle.corner) + { + isPeak = false; + break; + } + } + //plus方向寻找 + int plus_i = i; + while (1) + { + plus_i++; + if (plus_i >= i_max) + plus_i = 0; + SSG_dirCornerAngle& plus_dirAngle = peakCorners[plus_i]; + int pntDist_0 = plus_dirAngle.pntIdx - a_dirAngle.pntIdx; + if (pntDist_0 < 0) + pntDist_0 += cornerSize; + if (pntDist_0 > curr_dist2) + break; + + if (a_dirAngle.corner < plus_dirAngle.corner) + { + isPeak = false; + break; + } + } + if (true == isPeak) + { + double corner_curr = a_dirAngle.corner; + double corner_1 = corners[a_dirAngle.forward_pntIdx].corner; + double corner_2 = corners[a_dirAngle.backward_pntIdx].corner; + double diff_1 = corner_curr - corner_1; + double diff_2 = corner_curr - corner_2; + if ((diff_1 > corner_curr / 4) && (diff_2 > corner_curr / 4)) + { + double polarAngle = a_dirAngle.point.angle; + double angleDiff1 = abs(polarAngle - a_dirAngle.forwardAngle); + if (angleDiff1 > 180) + angleDiff1 = 360 - angleDiff1; + double angleDiff2 = abs(polarAngle - a_dirAngle.backwardAngle); + if (angleDiff2 > 180) + angleDiff2 = 360 - angleDiff2; + if ((angleDiff1 < 10) || (angleDiff2 < 10)) //过滤掉边与极线方向相近导致的波动 + { + if (a_dirAngle.corner < 160) //乱序产生角度错误 + { + a_dirAngle.flag = 0; + cornerPlusPeaks.push_back(a_dirAngle); + } + } + else + { + a_dirAngle.flag = 0; + cornerPlusPeaks.push_back(a_dirAngle); + } + } + } + } +} + //计算定位盘中心点位姿 SSX_pointPoseInfo sx_getLocationPlatePose( std::vector< std::vector>& scanLines, @@ -706,10 +943,20 @@ SSX_pointPoseInfo sx_getLocationPlatePose( //拟合平面 std::vector Points3ds; + std::vector polarPoints; for (int i = 0; i < (int)objClusters[objIdx].size(); i++) { cv::Point3f a_pt = cv::Point3f(objClusters[objIdx][i].pt3D.x, objClusters[objIdx][i].pt3D.y, objClusters[objIdx][i].pt3D.z); Points3ds.push_back(a_pt); + SWD_polarPt a_polarPt; + a_polarPt.lineIdx = objClusters[objIdx][i].nPointIdx >> 16; + a_polarPt.ptIdx = objClusters[objIdx][i].nPointIdx & 0x0000FFFF; + a_polarPt.R = 0; + a_polarPt.angle = 0; + a_polarPt.x = objClusters[objIdx][i].pt3D.x; + a_polarPt.y = objClusters[objIdx][i].pt3D.y; + a_polarPt.z = objClusters[objIdx][i].pt3D.z; + polarPoints.push_back(a_polarPt); } //计算面参数: z = Ax + By + C //res: [0]=A, [1]= B, [2]=-1.0, [3]=C, @@ -734,6 +981,9 @@ SSX_pointPoseInfo sx_getLocationPlatePose( double y = Points3ds[i].x * poseR.planeCalib[3] + Points3ds[i].y * poseR.planeCalib[4] + Points3ds[i].z * poseR.planeCalib[5]; double z = Points3ds[i].x * poseR.planeCalib[6] + Points3ds[i].y * poseR.planeCalib[7] + Points3ds[i].z * poseR.planeCalib[8]; projectPoints3ds[i] = cv::Point3f(x, y, z); + polarPoints[i].x = x; + polarPoints[i].y = y; + polarPoints[i].z = z; sum_x += x; sum_y += y; } @@ -751,6 +1001,90 @@ SSX_pointPoseInfo sx_getLocationPlatePose( double center_x = sum_x / (double)Points3ds.size(); double center_y = sum_y / (double)Points3ds.size(); + //计算极坐标的R和Theta + for (int pi = 0; pi < (int)polarPoints.size(); 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); + for (int pi = 0; pi < (int)polarPoints.size(); pi++) + polarPoints[pi].cptIndex = pi; // index + + //计算有序边缘点的角度变化 2026.03.06 + std::vector< SSG_dirCornerAngle> dirCornerAngles; + double dirAngleScale = 20.0; + _computeClosedPntListDirCorners(polarPoints, dirAngleScale, dirCornerAngles); + //提取方向角拐点(正)极值 + std::vector< SSG_dirCornerAngle> cornerPlusPeaks; + double minCutAngleTh = 30; + _searchPlusCornerPeaks(dirCornerAngles, minCutAngleTh, cornerPlusPeaks); + + //生成直线段数据 + int cornerIdx_0 = -1; + int cornerIdx_1 = -1; + for (int i = 0; i < (int)cornerPlusPeaks.size(); i++) + { + if ((cornerPlusPeaks[i].point.angle > 0) && (cornerPlusPeaks[i].point.angle < 90)) + cornerIdx_0 = i; + if ((cornerPlusPeaks[i].point.angle > 90) && (cornerPlusPeaks[i].point.angle < 180)) + cornerIdx_1 = i; + } + if ((cornerIdx_0 < 0) && (cornerIdx_1 < 0)) + { + if (objIdx < 0) + { + *errCode = SX_ERR_UNKNOWN_PLATE_DIR; + return resultPose; + } + } + std::vector lineFittingPts; + double endingRemoveDist = 10.0; + double validFittingLen = 100; + if (cornerIdx_0 >= 0) + { + int startIdx = cornerPlusPeaks[cornerIdx_0].pntIdx; + double sx = polarPoints[startIdx].x; + double sy = polarPoints[startIdx].y; + for (int m = startIdx; m < (int)polarPoints.size(); m++) + { + double dist = sqrt(pow(polarPoints[m].x - sx, 2) + pow(polarPoints[m].y - sy, 2)); + if ((dist >= endingRemoveDist) && (dist <= (validFittingLen + endingRemoveDist))) + { + SVzNL3DPoint a_fitPt = { polarPoints[m].x, polarPoints[m].y, polarPoints[m].z }; + lineFittingPts.push_back(a_fitPt); + } + if (dist > (validFittingLen + endingRemoveDist)) + break; + } + } + else + { + int startIdx = cornerPlusPeaks[cornerIdx_1].pntIdx; + double sx = polarPoints[startIdx].x; + double sy = polarPoints[startIdx].y; + for (int m = startIdx; m >= 0; m--) + { + double dist = sqrt(pow(polarPoints[m].x - sx, 2) + pow(polarPoints[m].y - sy, 2)); + if ((dist >= endingRemoveDist) && (dist <= (validFittingLen + endingRemoveDist))) + { + SVzNL3DPoint a_fitPt = { polarPoints[m].x, polarPoints[m].y, polarPoints[m].z }; + lineFittingPts.push_back(a_fitPt); + } + if (dist > (validFittingLen + endingRemoveDist)) + break; + } + } + //直线拟合 + double _k, _b; + lineFitting(lineFittingPts, &_k, &_b); + double normData = sqrt(pow(_k, 2) + 1); + SVzNL3DPoint xDir = { 1.0/normData, _k/normData, 0.0 }; + //计算4个点 SVzNL2DPointD top_pt = { center_x, center_y - centerZ_R }; SVzNL2DPointD bottom_pt = { center_x, center_y + centerZ_R }; @@ -834,8 +1168,12 @@ SSX_pointPoseInfo sx_getLocationPlatePose( double center_z = (ptTop.z + ptBtm.z + ptLeft.z + ptRight.z) / 4; resultPose.center = { center_x, center_y, center_z }; resultPose.xDir = { 0.0, 0.0, -1.0 }; // { 1.0, 0.0, 0 }; - resultPose.yDir = { 0.0, 1.0, 0 }; - resultPose.normalDir = { 1.0, 0.0, 0 };// { 0.0, 0.0, 1.0 }; + //resultPose.yDir = { 0.0, 1.0, 0 }; + resultPose.normalDir = xDir; // { 1.0, 0.0, 0 };// { 0.0, 0.0, 1.0 }; + //叉乘出y; + //向量叉乘 + resultPose.yDir = vec3_cross(resultPose.normalDir, resultPose.xDir); + //旋转回去 for (int i = 0; i < lineNum; i++) {