From 1026adaa5c5562113a9ecf5c4adb3e97c2576316 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Fri, 27 Jun 2025 23:04:51 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BB=A3=E7=A0=81=E5=A4=87=E4=BB=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/SG_baseFunc.cpp | 2 +- sourceCode/SG_sieveNodeDetection_Export.h | 2 +- sourceCode/sieveNodeDetection.cpp | 486 +++++++++++++++++++++- 3 files changed, 478 insertions(+), 12 deletions(-) diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 69b5c20..103bf31 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -957,7 +957,7 @@ SSG_planeCalibPara sg_getPlaneCalibPara( if (pkTop.size() < 1) return planePara; - int pntSizeTh = totalPntSize * 3 / 10; + int pntSizeTh = totalPntSize / 10; SSG_intPair* vldPeak = NULL; for (int i = 0, i_max = pkTop.size(); i < i_max; i++) { diff --git a/sourceCode/SG_sieveNodeDetection_Export.h b/sourceCode/SG_sieveNodeDetection_Export.h index 5246e16..7333e58 100644 --- a/sourceCode/SG_sieveNodeDetection_Export.h +++ b/sourceCode/SG_sieveNodeDetection_Export.h @@ -30,7 +30,7 @@ SG_APISHARED_EXPORT void sg_getSieveNodes( SVzNL3DLaserLine* laser3DPoints, int lineNum, const SSG_sieveNodeDetectionParam sieveDetectParam, - std::vector& nodePos); + std::vector>& nodePos); //计算一个平面调平参数。 //以数据输入中ROI以内的点进行平面拟合，计算调平参数 diff --git a/sourceCode/sieveNodeDetection.cpp b/sourceCode/sieveNodeDetection.cpp index 259ed89..318f1f1 100644 --- a/sourceCode/sieveNodeDetection.cpp +++ b/sourceCode/sieveNodeDetection.cpp @@ -5,6 +5,8 @@ #include #include +#define ALGO_USE_PATTERN_MATCH 0 + void sg_lineDataR(SVzNL3DLaserLine* a_line, const double* camPoseR, double groundH) @@ -141,12 +143,162 @@ SVzNL2DPoint _getNearestScanPt( return bestPos; } +//搜索目标在排序目标中的位置。返回的是排序目标前一位的序号，-1为最后一个 +int getObjPostPos(std::vector& sortedTrees, int treeID, std::vector& trees, bool HPos) +{ + SSG_semiCircleFeatureTree& obj_tree = trees[treeID]; + + for (int i = 0, i_max = sortedTrees.size(); i < i_max; i++) + { + int a_tree_id = sortedTrees[i]; + SSG_semiCircleFeatureTree& a_tree = trees[a_tree_id]; + if (((obj_tree.centerPt.x < a_tree.centerPt.x) && (true == HPos)) || + ((obj_tree.centerPt.y < a_tree.centerPt.y) && (false == HPos))) + { + return i; + } + } + return -1; //最后一个 +} + +typedef struct +{ + SVzNL2DPoint nodePos; + int LT_link; + int RT_link; + int LB_link; + int RB_link; + double LT_dist; + double RT_dist; + double RB_dist; + double LB_dist; +}NodeQuadLink; + +int getObjPostPos(std::vector& sortedLine, int nodeID, std::vector& nodeLink, bool HPos) +{ + SVzNL2DPoint& nodePos = nodeLink[nodeID].nodePos; + for (int i = 0, i_max = sortedLine.size(); i < i_max; i++) + { + int a_node_id = sortedLine[i]; + SVzNL2DPoint& sorting_node_pos = nodeLink[a_node_id].nodePos; + if (((nodePos.x < sorting_node_pos.x) && (true == HPos)) || + ((nodePos.y < sorting_node_pos.y) && (false == HPos))) + { + return i; + } + } + return -1; //最后一个 +} + + +void _recursiveSorting( + int nodeId, + std::vector>& hSorting, + std::vector< int>& nodeProcFlag, + std::vector< int>& nodeSortingLineID, + std::vector< NodeQuadLink>& nodeLinks, + bool HPos, + int* maxLineId) +{ + std::vector processBuff; + processBuff.push_back(nodeId); + while (processBuff.size() > 0) + { + int nodeId = processBuff.front(); + if (nodeProcFlag[nodeId] == 0) + { + int currLineId = nodeSortingLineID[nodeId]; + if (*maxLineId < currLineId) + *maxLineId = currLineId; + std::vector& currLine = hSorting[currLineId]; + //将当前Node插入currLine + int nodePos = getObjPostPos(currLine, nodeId, nodeLinks, HPos); + if (nodePos < 0) + currLine.push_back(nodeId); + else + currLine.insert(currLine.begin() + nodePos, nodeId); + nodeProcFlag[nodeId] = 1; + //检查Link + NodeQuadLink& currLink = nodeLinks[nodeId]; + if ((currLink.LT_dist > 0) && (currLineId > 0)) + { + int linkNode = currLink.LT_link; + int lineId = currLineId - 1; + nodeSortingLineID[linkNode] = lineId; + processBuff.push_back(linkNode); + currLink.LT_dist = -1.0; + nodeLinks[linkNode].RB_dist = -1.0; //取消 + } + if ((currLink.RT_dist > 0) && (currLineId > 0)) + { + int linkNode = currLink.RT_link; + int lineId = currLineId - 1; + nodeSortingLineID[linkNode] = lineId; + processBuff.push_back(linkNode); + currLink.RT_dist = -1.0; + nodeLinks[linkNode].LB_dist = -1.0; //取消 + } + if ((currLink.LB_dist > 0) && (currLineId < hSorting.size() - 1)) + { + int linkNode = currLink.LB_link; + int lineId = currLineId + 1; + nodeSortingLineID[linkNode] = lineId; + processBuff.push_back(linkNode); + currLink.LB_dist = -1.0; + nodeLinks[linkNode].RT_dist = -1.0; //取消 + } + if ((currLink.RB_dist > 0) && (currLineId < hSorting.size() - 1)) + { + int linkNode = currLink.RB_link; + int lineId = currLineId + 1; + nodeSortingLineID[linkNode] = lineId; + processBuff.push_back(linkNode); + currLink.RB_dist = -1.0; + nodeLinks[linkNode].LT_dist = -1.0; //取消 + } + } + processBuff.erase(processBuff.begin()); + } +} + +void _lineFillNullNode( + std::vector& sortingNodes_line, + std::vector< NodeQuadLink>& nodeLinks, + const double lineNodeDistMean +) +{ + for (int i = 0; i < sortingNodes_line.size() -1; i++) + { + SVzNL3DPosition node_1 = sortingNodes_line[i]; + if (node_1.nPointIdx < 0) + continue; + SVzNL3DPosition node_2 = sortingNodes_line[i + 1]; + double dist = sqrt(pow(node_1.pt3D.x - node_2.pt3D.x, 2) + pow(node_1.pt3D.y - node_2.pt3D.y, 2)); + int n = (int)(dist / lineNodeDistMean + 0.5) - 1; + if (n > 0) + { + //判断遗漏几个目标 + double x_step = (node_2.pt3D.x - node_1.pt3D.x) / (n + 1); + double y_step = (node_2.pt3D.y - node_1.pt3D.y) / (n + 1); + double z_step = (node_2.pt3D.z - node_1.pt3D.z) / (n + 1); + for (int m = 0; m < n; m++) + { + SVzNL3DPosition a_newNode; + a_newNode.nPointIdx = -1; + a_newNode.pt3D.x = x_step * (m + 1) + node_1.pt3D.x; + a_newNode.pt3D.y = y_step * (m + 1) + node_1.pt3D.y; + a_newNode.pt3D.z = z_step * (m + 1) + node_1.pt3D.z; + sortingNodes_line.insert(sortingNodes_line.begin() + i + 1 + m, a_newNode); + } + } + } +} void sg_getSieveNodes( SVzNL3DLaserLine* laser3DPoints, int lineNum, const SSG_sieveNodeDetectionParam sieveDetectParam, - std::vector& nodePos) + std::vector>& nodePos) { const int hole_max_ptSize = 20; int errCode = 0; @@ -287,19 +439,332 @@ void sg_getSieveNodes( } + //按行排序 - // (1)建立2D索引实现有序搜索 - //（2）从 - std::vector> sortedTrees; +#if 1 + // (1)量化到2D,建立2D索引实现有序搜索 + SVzNL3DRangeD ROI = sg_getScanDataROI(laser3DPoints, lineNum); + double scale_x = 1.0; + double scale_y = 1.0; + int rows = (int)((ROI.yRange.max - ROI.yRange.min) / scale_y) + 1; + int cols = (int)((ROI.xRange.max - ROI.xRange.min) / scale_x) + 1; + if ((rows == 0) || (cols == 0)) + return; + cv::Mat objMask = cv::Mat(rows, cols, CV_32SC1, -1);//rows, cols for (int i = 0, i_max = stopTrees.size(); i < i_max; i++) { - //if(stopTrees[i].) + SVzNL3DPoint treeCenter = laser3DPoints[stopTrees[i].centerPos.x].p3DPosition[stopTrees[i].centerPos.y].pt3D; + int px = (int)((treeCenter.x - ROI.xRange.min) / scale_x); + int py = (int)((treeCenter.y - ROI.yRange.min) / scale_y); + objMask.at(py, px) = i; + } +#endif + + std::vector< NodeQuadLink> nodeLinks(stopTrees.size(), { {0,0},-1,-1,-1,-1, -1.0, -1.0, -1.0, -1.0}); + double neighbourDistTh = sieveDetectParam.sieveHoleSize * 1.2; //以正方形为模型，边长为1，则对角线为1.414，取1.2为长度门限 + for (int i = 0, i_max = stopTrees.size(); i < i_max; i++) + { + SSG_semiCircleFeatureTree& tree_1 = stopTrees[i]; + NodeQuadLink& link_1 = nodeLinks[i]; + link_1.nodePos = tree_1.centerPos; + for (int j = i + 1; j < i_max; j++) + { + SSG_semiCircleFeatureTree& tree_2 = stopTrees[j]; + NodeQuadLink& link_2 = nodeLinks[j]; + double dist = sqrt(pow(tree_1.centerPt.x - tree_2.centerPt.x, 2) + pow(tree_1.centerPt.y - tree_2.centerPt.y, 2)); + if (dist < neighbourDistTh) + { + if (tree_1.centerPt.x < tree_2.centerPt.x) //left + { + if (tree_1.centerPt.y < tree_2.centerPt.y) //top + { + //tree1:RB link is tree_2 + //tree2:LT link is tree_1 + if ( (link_1.RB_link >= 0) || (link_2.LT_link >= 0)) + { + bool currIsBest = true; + //判断最佳关系 + if ((link_1.RB_link >= 0) && (dist > link_1.RB_dist)) + currIsBest = false; + if ((link_2.LT_link >= 0) && (dist > link_2.LT_dist)) + currIsBest = false; + if (true == currIsBest) + { + if (link_1.RB_link >= 0) + { + nodeLinks[link_1.RB_link].LT_link = -1; + nodeLinks[link_1.RB_link].LT_dist = 0; + } + link_1.RB_link = j; + link_1.RB_dist = dist; + if (link_2.LT_link >= 0) + { + nodeLinks[link_2.LT_link].RB_link = -1; + nodeLinks[link_2.LT_link].RB_dist = 0; + } + link_2.LT_link = i; + link_2.LT_dist = dist; + } + } + else + { + link_1.RB_link = j; + link_1.RB_dist = dist; + link_2.LT_link = i; + link_2.LT_dist = dist; + } + + } + else//bottom + { + //tree1:RT link is tree_2 + //tree2:LB link is tree_1 + if ((link_1.RT_link >= 0) || (link_2.LB_link >= 0)) + { + bool currIsBest = true; + //判断最佳关系 + if ((link_1.RT_link >= 0) && (dist > link_1.RT_dist)) + currIsBest = false; + if ((link_2.LB_link >= 0) && (dist > link_2.LB_dist)) + currIsBest = false; + if (true == currIsBest) + { + if (link_1.RT_link >= 0) + { + nodeLinks[link_1.RT_link].LB_link = -1; + nodeLinks[link_1.RT_link].LB_dist = 0; + } + link_1.RT_link = j; + link_1.RT_dist = dist; + if (link_2.LB_link >= 0) + { + nodeLinks[link_2.LB_link].RT_link = -1; + nodeLinks[link_2.LB_link].RT_dist = 0; + } + link_2.LB_link = i; + link_2.LB_dist = dist; + } + } + else + { + link_1.RT_link = j; + link_1.RT_dist = dist; + link_2.LB_link = i; + link_2.LB_dist = dist; + } + } + } + else //right + { + if (tree_1.centerPt.y < tree_2.centerPt.y) //top + { + //tree1:LB link is tree_2 + //tree2:RT link is tree_1 + if ((link_1.LB_link >= 0) || (link_2.RT_link >= 0)) + { + bool currIsBest = true; + //判断最佳关系 + if ((link_1.LB_link >= 0) && (dist > link_1.LB_dist)) + currIsBest = false; + if ((link_2.RT_link >= 0) && (dist > link_2.RT_dist)) + currIsBest = false; + if (true == currIsBest) + { + if (link_1.LB_link >= 0) + { + nodeLinks[link_1.LB_link].RT_link = -1; + nodeLinks[link_1.LB_link].RT_dist = 0; + } + link_1.LB_link = j; + link_1.LB_dist = dist; + if (link_2.RT_link >= 0) + { + nodeLinks[link_2.RT_link].LB_link = -1; + nodeLinks[link_2.RT_link].LB_dist = 0; + } + link_2.RT_link = i; + link_2.RT_dist = dist; + } + } + else + { + link_1.LB_link = j; + link_1.LB_dist = dist; + link_2.RT_link = i; + link_2.RT_dist = dist; + } + } + else //bottom + { + //tree1:LT link is tree_2 + //tree2:RB link is tree_1 + if ((link_1.LT_link >= 0) || (link_2.RB_link >= 0)) + { + bool currIsBest = true; + //判断最佳关系 + if ((link_1.LT_link >= 0) && (dist > link_1.LT_dist)) + currIsBest = false; + if ((link_2.RB_link >= 0) && (dist > link_2.RB_dist)) + currIsBest = false; + if (true == currIsBest) + { + if (link_1.LT_link >= 0) + { + nodeLinks[link_1.LT_link].RB_link = -1; + nodeLinks[link_1.LT_link].RB_dist = 0; + } + link_1.LT_link = j; + link_1.LT_dist = dist; + if (link_2.RB_link >= 0) + { + nodeLinks[link_2.RB_link].LT_link = -1; + nodeLinks[link_2.RB_link].LT_dist = 0; + } + link_2.RB_link = i; + link_2.RB_dist = dist; + } + } + else + { + link_1.LT_link = j; + link_1.LT_dist = dist; + link_2.RB_link = i; + link_2.RB_dist = dist; + } + } + } + } + } + } + //（2）建立节点间的相互关系 + std::vector< int> nodeSortingLineID(stopTrees.size(), -1 ); //排序后的行位置 + std::vector< int> nodeProcFlag(stopTrees.size(), 0); //已经处理标志，”1“已经处理 + std::vector> hSorting; + hSorting.resize(stopTrees.size()); + int startSortingLine = -1; //第一个检查的Node使用最中间的排序行，这样可以向上和向下添加行 + int gMaxLineId = 0; + for (int y = 0; y < rows; y++) + { + for (int x = 0; x < cols; x++) + { + int nodeId = objMask.at(y, x); + if (nodeId >= 0) + { + if (nodeProcFlag[nodeId] > 0) + continue; + + std::vector processBuff; + processBuff.push_back(nodeId); + if (startSortingLine < 0) + startSortingLine = 0; + else + { + //确定行序号 + startSortingLine = 0; //TBD + + } + nodeSortingLineID[nodeId] = startSortingLine; + int maxLineId = startSortingLine; + _recursiveSorting( + nodeId, + hSorting, + nodeProcFlag, + nodeSortingLineID, + nodeLinks, + true, + &maxLineId); + if (gMaxLineId < maxLineId) + gMaxLineId = maxLineId; + } + } + } + //计算行Node的距离均值 + double lineNodeDist = 0; + int lineNodeDistNum = 0; + for (int i = 0, i_max = stopTrees.size(); i < i_max; i++) + { + NodeQuadLink& link_1 = nodeLinks[i]; + if (link_1.LB_link >= 0) + { + if (nodeLinks[link_1.LB_link].LT_link >= 0) + { + int id2 = nodeLinks[link_1.LB_link].LT_link; + double dist = sqrt(pow(stopTrees[i].centerPt.x - stopTrees[id2].centerPt.x, 2) + pow(stopTrees[i].centerPt.y - stopTrees[id2].centerPt.y, 2)); + lineNodeDist += dist; + lineNodeDistNum++; + } + } + if (link_1.LT_link >= 0) + { + if (nodeLinks[link_1.LT_link].LB_link >= 0) + { + int id2 = nodeLinks[link_1.LT_link].LB_link; + double dist = sqrt(pow(stopTrees[i].centerPt.x - stopTrees[id2].centerPt.x, 2) + pow(stopTrees[i].centerPt.y - stopTrees[id2].centerPt.y, 2)); + lineNodeDist += dist; + lineNodeDistNum++; + } + } + if (link_1.RB_link >= 0) + { + if (nodeLinks[link_1.RB_link].RT_link >= 0) + { + int id2 = nodeLinks[link_1.RB_link].RT_link; + double dist = sqrt(pow(stopTrees[i].centerPt.x - stopTrees[id2].centerPt.x, 2) + pow(stopTrees[i].centerPt.y - stopTrees[id2].centerPt.y, 2)); + lineNodeDist += dist; + lineNodeDistNum++; + } + } + if (link_1.RT_link >= 0) + { + if (nodeLinks[link_1.RT_link].RB_link >= 0) + { + int id2 = nodeLinks[link_1.RT_link].RB_link; + double dist = sqrt(pow(stopTrees[i].centerPt.x - stopTrees[id2].centerPt.x, 2) + pow(stopTrees[i].centerPt.y - stopTrees[id2].centerPt.y, 2)); + lineNodeDist += dist; + lineNodeDistNum++; + } + } + } + if (lineNodeDistNum > 0) + lineNodeDist = lineNodeDist / lineNodeDistNum; + + //生成目标坐标点,遗漏点检查 + std::vector> sortingNodes; + for (int i = 0; i <= gMaxLineId; i++) + { + std::vector& a_sortingLine = hSorting[i]; + std::vector sortingNodes_line; + for (int j = 0, j_max = a_sortingLine.size(); j < j_max; j++) + { + int nodeId = a_sortingLine[j]; + SVzNL3DPosition a_pt; + a_pt.nPointIdx = nodeId; + a_pt.pt3D = stopTrees[nodeId].centerPt; + sortingNodes_line.push_back(a_pt); + } + //遗漏点检查 + _lineFillNullNode( sortingNodes_line, nodeLinks, lineNodeDist ); + sortingNodes.push_back(sortingNodes_line); } - //遗漏点检查 - - //迭代生长 + //结果数据第一行和最后一行如果不完整，不使用 + if (sortingNodes.size() > 3) + { + if (sortingNodes[0].size() < sortingNodes[2].size()) + sortingNodes.erase(sortingNodes.begin()); + } + //输出 + for (int i = 0, i_max = sortingNodes.size(); i < i_max; i++) + { + std::vector& sortingNodes_line = sortingNodes[i]; + if (sortingNodes_line.size() == 0) + break; + std::vector resultLine; + for (int j = 0, j_max = sortingNodes_line.size(); j < j_max; j++) + resultLine.push_back(sortingNodes_line[j].pt3D); + nodePos.push_back(resultLine); + } #if _OUTPUT_LINE_PROC_RESULT //输出扫描线处理结果 for (int i = 0, i_max = stopTrees.size(); i < i_max; i++) @@ -309,13 +774,14 @@ void sg_getSieveNodes( { SSG_featureSemiCircle& a_feature = a_tree_features[j]; for (int m = a_feature.startPtIdx; m <= a_feature.endPtIdx; m++) - laser3DPoints[a_feature.lineIdx].p3DPosition[m].nPointIdx = 1; //此处nPointIdx转义 - + laser3DPoints[a_feature.lineIdx].p3DPosition[m].nPointIdx = 0; //1; //此处nPointIdx转义 +#if 0 laser3DPoints[a_feature.lineIdx].p3DPosition[a_feature.midPtIdx].nPointIdx = 2; if(stopTrees[i].treeType == 0) laser3DPoints[stopTrees[i].centerPos.x].p3DPosition[stopTrees[i].centerPos.y].nPointIdx = 3; else laser3DPoints[stopTrees[i].centerPos.x].p3DPosition[stopTrees[i].centerPos.y].nPointIdx = 4; +#endif } }