jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
algoLib/sourceCode/sieveNodeDetection.cpp

802 lines
23 KiB
C++
Raw Normal View History

source code
2025-06-08 10:46:41 +08:00
#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "SG_sieveNodeDetection_Export.h"
#include <opencv2/opencv.hpp>
#include <limits>
代码备份
2025-06-27 23:04:51 +08:00
#define ALGO_USE_PATTERN_MATCH 0
source code
2025-06-08 10:46:41 +08:00
void sg_lineDataR(SVzNL3DLaserLine* a_line,
const double* camPoseR,
double groundH)
{
lineDataRT(a_line, camPoseR, groundH);
}
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
#if 0
source code
2025-06-08 10:46:41 +08:00
//ɨ<><C9A8><EFBFBD>ߴ<EFBFBD><DFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void sg_sieveNodeDetection_lineProc(
SVzNL3DLaserLine* a_line,
int lineIdx,
int* errCode,
std::vector<std::vector< SSG_featureSemiCircle>>& all_vLineFeatures,
std::vector<std::vector<int>>& noisePts,
const SSG_sieveNodeDetectionParam sieveDetectParam)
{
std::vector< SSG_featureSemiCircle> a_line_features;
//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
std::vector<SVzNL3DPosition> filterData;
std::vector<int> lineNoisePts;
sg_lineDataRemoveOutlier(a_line->p3DPosition, a_line->nPositionCnt, sieveDetectParam.filterParam, filterData, lineNoisePts);
noisePts.push_back(lineNoisePts);
sg_getLineUpperSemiCircleFeature(
filterData.data(),
filterData.size(),
lineIdx,
sieveDetectParam.sieveDiameter,
sieveDetectParam.slopeParam,
a_line_features);
all_vLineFeatures.push_back(a_line_features); //<2F><><EFBFBD><EFBFBD>Ҳ<EFBFBD><D2B2><EFBFBD>룬<EFBFBD><EBA3AC>֤<EFBFBD>ܰ<EFBFBD><DCB0>к<EFBFBD><D0BA><EFBFBD><EFBFBD><EFBFBD>
return;
}
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
#endif
source code
2025-06-08 10:46:41 +08:00
int _checkFeatureSplit(
SSG_featureSemiCircle& a_feaurue,
std::vector< SSG_featureSemiCircle>& chk_line_feature,
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
double splitMinDist,
source code
2025-06-08 10:46:41 +08:00
double splitMaxDist) //<2F>ڴ˾<DAB4><CBBE><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>Ч<EFBFBD>ֲ<EFBFBD>
{
int split = -1;
for (int i = 0, i_max = chk_line_feature.size(); i < i_max; i++)
{
if (i < i_max - 1)
{
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
if ((chk_line_feature[i].midPt.y < a_feaurue.midPt.y) && (chk_line_feature[i + 1].midPt.y > a_feaurue.midPt.y))
source code
2025-06-08 10:46:41 +08:00
{
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
double dist_1 = abs(chk_line_feature[i].midPt.y - a_feaurue.midPt.y);
double dist_2 = abs(chk_line_feature[i+1].midPt.y - a_feaurue.midPt.y);
if ((dist_1 > splitMinDist) && (dist_1 < splitMaxDist) && (dist_2 > splitMinDist)&& (dist_2 < splitMaxDist))
source code
2025-06-08 10:46:41 +08:00
{
split = i;
break;
}
}
}
}
return split;
}
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
bool compareByWidth(const SSG_featureSemiCircle& a, const SSG_featureSemiCircle& b) {
return a.width < b.width;
}
SVzNL3DPoint _getMeanPt(
std::vector<SSG_featureSemiCircle>& nodes,
SVzNL3DLaserLine* laser3DPoints)
{
int nodeSize = nodes.size();
int num = 0;
SVzNL3DPoint meanPt = { 0.0,0.0,0.0 };
for (int i = 0; i < nodeSize; i++)
{
SSG_featureSemiCircle& a_node = nodes[i];
int lineIdx = a_node.lineIdx;
for (int j = a_node.startPtIdx; j <= a_node.endPtIdx; j++)
{
if (laser3DPoints[lineIdx].p3DPosition[j].pt3D.z > 1e-4)
{
num++;
meanPt.x += laser3DPoints[lineIdx].p3DPosition[j].pt3D.x;
meanPt.y += laser3DPoints[lineIdx].p3DPosition[j].pt3D.y;
meanPt.z += laser3DPoints[lineIdx].p3DPosition[j].pt3D.z;
}
}
}
if (num > 0)
{
meanPt.x = meanPt.x / num;
meanPt.y = meanPt.y / num;
meanPt.z = meanPt.z / num;
}
return meanPt;
}
SVzNL2DPoint _getNearestScanPt(
std::vector<SSG_featureSemiCircle>& nodes,
SVzNL3DLaserLine* laser3DPoints,
SVzNL3DPoint objPt)
{
int nodeSize = nodes.size();
SVzNL2DPoint bestPos = { -1, -1 };
double minDist = -1;
for (int i = 0; i < nodeSize; i++)
{
SSG_featureSemiCircle& a_node = nodes[i];
int lineIdx = a_node.lineIdx;
for (int j = a_node.startPtIdx; j <= a_node.endPtIdx; j++)
{
if (laser3DPoints[lineIdx].p3DPosition[j].pt3D.z > 1e-4)
{
SVzNL3DPoint& a_pt = laser3DPoints[lineIdx].p3DPosition[j].pt3D;
double dist = sqrt(pow(a_pt.x - objPt.x, 2) + pow(a_pt.y - objPt.y, 2));
if (minDist < 0)
{
minDist = dist;
bestPos = { a_node.lineIdx, j };
}
else
{
if (minDist > dist)
{
minDist = dist;
bestPos = { a_node.lineIdx, j };
}
}
}
}
}
return bestPos;
}
代码备份
2025-06-27 23:04:51 +08:00
//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>е<EFBFBD>λ<EFBFBD>á<EFBFBD><C3A1><EFBFBD><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>ǰһλ<D2BB><CEBB><EFBFBD><EFBFBD><EFBFBD>ţ<EFBFBD>-1Ϊ<31><CEAA><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
int getObjPostPos(std::vector<int>& sortedTrees, int treeID, std::vector<SSG_semiCircleFeatureTree>& 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; //<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
}
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<int>& sortedLine, int nodeID, std::vector<NodeQuadLink>& 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; //<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
}
void _recursiveSorting(
int nodeId,
std::vector<std::vector<int>>& hSorting,
std::vector< int>& nodeProcFlag,
std::vector< int>& nodeSortingLineID,
std::vector< NodeQuadLink>& nodeLinks,
bool HPos,
int* maxLineId)
{
std::vector<int> 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<int>& currLine = hSorting[currLineId];
//<2F><><EFBFBD><EFBFBD>ǰNode<64><65><EFBFBD><EFBFBD>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;
//<2F><><EFBFBD><EFBFBD>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; //ȡ<><C8A1>
}
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; //ȡ<><C8A1>
}
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; //ȡ<><C8A1>
}
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; //ȡ<><C8A1>
}
}
processBuff.erase(processBuff.begin());
}
}
void _lineFillNullNode(
std::vector<SVzNL3DPosition>& 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)
{
//<2F>ж<EFBFBD><D0B6><EFBFBD>©<EFBFBD><C2A9><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>
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);
}
}
}
}
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
source code
2025-06-08 10:46:41 +08:00
void sg_getSieveNodes(
SVzNL3DLaserLine* laser3DPoints,
int lineNum,
const SSG_sieveNodeDetectionParam sieveDetectParam,
代码备份
2025-06-27 23:04:51 +08:00
std::vector<std::vector<SVzNL3DPoint>>& nodePos)
source code
2025-06-08 10:46:41 +08:00
{
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
const int hole_max_ptSize = 20;
source code
2025-06-08 10:46:41 +08:00
int errCode = 0;
for (int i = 0; i < lineNum; i++)
{
if (i == 19)
int kkk = 1;
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
sg_lineDataRemoveOutlier_changeOriginData(
laser3DPoints[i].p3DPosition,
laser3DPoints[i].nPositionCnt,
sieveDetectParam.filterParam);
source code
2025-06-08 10:46:41 +08:00
//<2F><>nPointIdxת<78><D7AA>ʹ<EFBFBD><CAB9>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>
for (int j = 0; j < laser3DPoints[i].nPositionCnt; j++)
laser3DPoints[i].p3DPosition[j].nPointIdx = 0;
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
}
//<2F>׶<EFBFBD><D7B6><EFBFBD><EFBFBD>⣬<EFBFBD><E2A3AC>С<EFBFBD>Ŀ׶<C4BF><D7B6><EFBFBD>Ҫ<EFBFBD>ϲ<EFBFBD>
int maskY = laser3DPoints[0].nPositionCnt;
int maskX = lineNum;
//<2F><><EFBFBD>ɿ׶<C9BF><D7B6><EFBFBD>עMask<73><6B><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD>ע
cv::Mat bwImg = cv::Mat::zeros(maskY, maskX, CV_8UC1);//rows, cols
for (int i = 0; i < lineNum; i++)
{
for (int j = 0; j < laser3DPoints[i].nPositionCnt; j++)
{
if(laser3DPoints[i].p3DPosition[j].pt3D.z < 1e-4)
bwImg.at<uchar>(j, i) = 1;
}
}
//<2F>׶<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD>ע
cv::Mat labImg;
std::vector<SSG_Region> labelRgns;
SG_TwoPassLabel(bwImg, labImg, labelRgns, 8);
//<2F><><EFBFBD>׶<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>б<EFBFBD>ʶ
cv::Mat holeMask = cv::Mat::zeros(maskY, maskX, CV_32SC1);//rows, cols
for (int i = 0, i_max = labelRgns.size(); i < i_max; i++)
{
int rgnID = labelRgns[i].labelID;
if (labelRgns[i].ptCounter < hole_max_ptSize) //<2F>׶<EFBFBD>
{
for (int m = labelRgns[i].roi.left; m <= labelRgns[i].roi.right; m++)
{
for (int n = labelRgns[i].roi.top; n <= labelRgns[i].roi.bottom; n++)
{
if (rgnID == labImg.at<int>(n, m))
holeMask.at<int>(n, m) = rgnID;
}
}
}
}
#if _OUTPUT_LINE_PROC_RESULT
cv::Mat holeMaskImage;
cv::normalize(holeMask, holeMaskImage, 0, 255, cv::NORM_MINMAX, CV_8U);
cv::imwrite("holeMask.png", holeMaskImage);
#endif
std::vector<std::vector< SSG_featureSemiCircle>> all_vLineFeatures;
for (int i = 0; i < lineNum; i++)
{
std::vector< SSG_featureSemiCircle> a_line_features;
sg_getLineUpperSemiCircleFeature(
laser3DPoints[i].p3DPosition,
laser3DPoints[i].nPositionCnt,
source code
2025-06-08 10:46:41 +08:00
i,
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
sieveDetectParam.sieveDiameter,
sieveDetectParam.slopeParam,
a_line_features,
holeMask);
//<2F><>nPointIdxת<78><D7AA>ʹ<EFBFBD><CAB9>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>
for (int j = 0; j < laser3DPoints[i].nPositionCnt; j++)
laser3DPoints[i].p3DPosition[j].nPointIdx = 0;
all_vLineFeatures.push_back(a_line_features); //<2F><><EFBFBD><EFBFBD>Ҳ<EFBFBD><D2B2><EFBFBD>룬<EFBFBD><EBA3AC>֤<EFBFBD>ܰ<EFBFBD><DCB0>к<EFBFBD><D0BA><EFBFBD><EFBFBD><EFBFBD>
source code
2025-06-08 10:46:41 +08:00
}
//<2F><><EFBFBD><EFBFBD>ɸ<EFBFBD><C9B8><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>feature<72><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>feature<72><65><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ɨ<EFBFBD><C9A8><EFBFBD>߱<EFBFBD><DFB1>ϲ<EFBFBD><CFB2><EFBFBD>һ<EFBFBD><D2BB>featureʱ<65><CAB1>˵<EFBFBD><CBB5><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ɨ<EFBFBD><C9A8><EFBFBD>ߵ<EFBFBD><DFB5><EFBFBD><EFBFBD><EFBFBD>feature<72><65><EFBFBD><EFBFBD>Чfeature<72><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD>feature<72><65>Ϊ<EFBFBD><CEAA>Чfeature
for (int i = 0; i < lineNum; i++)
{
//<2F><>ǰһ<C7B0><D2BB>ɨ<EFBFBD><C9A8><EFBFBD>߱Ƚ<DFB1>,Ѱ<>ҿ<EFBFBD>ʼ
std::vector< SSG_featureSemiCircle>& line_features = all_vLineFeatures[i];
if (i > 0)
{
std::vector< SSG_featureSemiCircle>& pre_line_features = all_vLineFeatures[i-1];
for (int j = 0, j_max = line_features.size(); j < j_max; j++)
{
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
int split = _checkFeatureSplit(line_features[j], pre_line_features, sieveDetectParam.sieveDiameter/2, sieveDetectParam.sieveHoleSize);
source code
2025-06-08 10:46:41 +08:00
if (split >= 0)
{
pre_line_features[split].flag = FEATURE_FLAG_INVLD_END;
pre_line_features[split + 1].flag = FEATURE_FLAG_INVLD_END;
line_features[j].flag = FEATURE_FLAG_VALID_START;
}
}
}
//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ɨ<EFBFBD><C9A8><EFBFBD>߱Ƚ<DFB1>,Ѱ<>ҽ<EFBFBD><D2BD><EFBFBD>
if (i < lineNum - 1)
{
std::vector< SSG_featureSemiCircle>& post_line_features = all_vLineFeatures[i + 1];
for (int j = 0, j_max = line_features.size(); j < j_max; j++)
{
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
int split = _checkFeatureSplit(line_features[j], post_line_features, sieveDetectParam.sieveDiameter/2, sieveDetectParam.sieveHoleSize);
source code
2025-06-08 10:46:41 +08:00
if (split >= 0)
{
post_line_features[split].flag = FEATURE_FLAG_INVLD_START;
post_line_features[split + 1].flag = FEATURE_FLAG_INVLD_START;
line_features[j].flag = FEATURE_FLAG_VALID_END;
}
}
}
}
//feature<72><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Чfeature<72><65><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㡣<EFBFBD><E3A1A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD>feature<72><65>Ϊ<EFBFBD><CEAA>Чfeature
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
std::vector<SSG_semiCircleFeatureTree> trees;
std::vector<SSG_semiCircleFeatureTree> stopTrees; //ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<SSG_semiCircleFeatureTree> invalidTrees; //<2F><><EFBFBD>Ƴ<EFBFBD><C6B3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Щ<EFBFBD><D0A9><EFBFBD><EFBFBD><EFBFBD>ܽ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>ֳɶ<D6B3><C9B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӷ<EFBFBD><D3B6><EFBFBD><EFBFBD>Ƴ<EFBFBD><C6B3><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
for (int i = 0; i < lineNum; i++)
{
//<2F><>ǰһ<C7B0><D2BB>ɨ<EFBFBD><C9A8><EFBFBD>߱Ƚ<DFB1>,Ѱ<>ҿ<EFBFBD>ʼ
std::vector< SSG_featureSemiCircle>& line_features = all_vLineFeatures[i];
sg_getFeatureGrowingTrees_semiCircle(
line_features,
i,
lineNum,
trees,
stopTrees,
invalidTrees,
sieveDetectParam.growParam);
}
//<2F><>ȷȷ<C8B7><C8B7><EFBFBD><EFBFBD><EFBFBD>ӵ<EFBFBD>
for (int i = 0, i_max = stopTrees.size(); i < i_max; i++)
{
//<2F><><EFBFBD>Ӷ<EFBFBD>ΪΪ<CEAA><CEAA><EFBFBD>е<EFBFBD><D0B5>ģ<EFBFBD>x,y,z)<29><>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD>
SSG_semiCircleFeatureTree& a_tree = stopTrees[i];
a_tree.centerPt = _getMeanPt(a_tree.treeNodes, laser3DPoints);
a_tree.centerPos = _getNearestScanPt(a_tree.treeNodes, laser3DPoints, a_tree.centerPt);
//<2F>жϺ<D0B6><CFBA><EFBFBD><EFBFBD>Ƿ񱻺<C7B7><F1B1BBBA>ӹ<EFBFBD><D3B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><D0BC><EFBFBD><EFBFBD>ĸ߶<C4B8><DFB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD><DFB5>м<EFBFBD><D0BC>߶ȱȽ<C8B1>
source code
2025-06-08 10:46:41 +08:00
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
}
代码备份
2025-06-27 23:04:51 +08:00
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
代码备份
2025-06-27 23:04:51 +08:00
#if 1
// (1)<29><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2D,<2C><><EFBFBD><EFBFBD>2D<32><44><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
for (int i = 0, i_max = stopTrees.size(); i < i_max; i++)
{
代码备份
2025-06-27 23:04:51 +08:00
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<int>(py, px) = i;
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
}
代码备份
2025-06-27 23:04:51 +08:00
#endif
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
代码备份
2025-06-27 23:04:51 +08:00
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; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊģ<CEAA>ͣ<EFBFBD><CDA3>߳<EFBFBD>Ϊ1<CEAA><31><EFBFBD><EFBFBD><EFBFBD>Խ<EFBFBD><D4BD><EFBFBD>Ϊ1.414<EFBFBD><EFBFBD>ȡ1.2Ϊ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ѹ<EFBFBD>ϵ
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;
}
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
代码备份
2025-06-27 23:04:51 +08:00
}
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;
//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ѹ<EFBFBD>ϵ
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;
//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ѹ<EFBFBD>ϵ
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;
//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ѹ<EFBFBD>ϵ
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;
}
}
}
}
}
}
//<2F><>2<EFBFBD><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>໥<EFBFBD><E0BBA5>ϵ
std::vector< int> nodeSortingLineID(stopTrees.size(), -1 ); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
std::vector< int> nodeProcFlag(stopTrees.size(), 0); //<2F>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><D6BE><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>
std::vector<std::vector<int>> hSorting;
hSorting.resize(stopTrees.size());
int startSortingLine = -1; //<2F><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Nodeʹ<65><CAB9><EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><D0BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϻ<EFBFBD><CFBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int gMaxLineId = 0;
for (int y = 0; y < rows; y++)
{
for (int x = 0; x < cols; x++)
{
int nodeId = objMask.at<int>(y, x);
if (nodeId >= 0)
{
if (nodeProcFlag[nodeId] > 0)
continue;
std::vector<int> processBuff;
processBuff.push_back(nodeId);
if (startSortingLine < 0)
startSortingLine = 0;
else
{
//ȷ<><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
startSortingLine = 0; //TBD
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
代码备份
2025-06-27 23:04:51 +08:00
}
nodeSortingLineID[nodeId] = startSortingLine;
int maxLineId = startSortingLine;
_recursiveSorting(
nodeId,
hSorting,
nodeProcFlag,
nodeSortingLineID,
nodeLinks,
true,
&maxLineId);
if (gMaxLineId < maxLineId)
gMaxLineId = maxLineId;
}
}
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Node<64>ľ<EFBFBD><C4BE><EFBFBD><EFBFBD><EFBFBD>ֵ
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;
//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><>©<EFBFBD><C2A9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<std::vector<SVzNL3DPosition>> sortingNodes;
for (int i = 0; i <= gMaxLineId; i++)
{
std::vector<int>& a_sortingLine = hSorting[i];
std::vector<SVzNL3DPosition> 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);
}
//<2F><>©<EFBFBD><C2A9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
_lineFillNullNode( sortingNodes_line, nodeLinks, lineNodeDist );
sortingNodes.push_back(sortingNodes_line);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݵ<EFBFBD>һ<EFBFBD>к<EFBFBD><D0BA><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9>
if (sortingNodes.size() > 3)
{
if (sortingNodes[0].size() < sortingNodes[2].size())
sortingNodes.erase(sortingNodes.begin());
}
//<2F><><EFBFBD><EFBFBD>
for (int i = 0, i_max = sortingNodes.size(); i < i_max; i++)
{
std::vector<SVzNL3DPosition>& sortingNodes_line = sortingNodes[i];
if (sortingNodes_line.size() == 0)
break;
std::vector<SVzNL3DPoint> 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);
}
source code
2025-06-08 10:46:41 +08:00
#if _OUTPUT_LINE_PROC_RESULT
//<2F><><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>ߴ<EFBFBD><DFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
for (int i = 0, i_max = stopTrees.size(); i < i_max; i++)
source code
2025-06-08 10:46:41 +08:00
{
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
std::vector< SSG_featureSemiCircle>& a_tree_features = stopTrees[i].treeNodes;
for (int j = 0, j_max = a_tree_features.size(); j < j_max; j++)
source code
2025-06-08 10:46:41 +08:00
{
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
SSG_featureSemiCircle& a_feature = a_tree_features[j];
source code
2025-06-08 10:46:41 +08:00
for (int m = a_feature.startPtIdx; m <= a_feature.endPtIdx; m++)
代码备份
2025-06-27 23:04:51 +08:00
laser3DPoints[a_feature.lineIdx].p3DPosition[m].nPointIdx = 0; //1; //<2F>˴<EFBFBD>nPointIdxת<78><D7AA>
#if 0
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
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;
代码备份
2025-06-27 23:04:51 +08:00
#endif
1. 修改了第三方库目录，相应修改了工程文件 2. 筛网工程开发中相应文件更新
2025-06-11 22:14:52 +08:00
source code
2025-06-08 10:46:41 +08:00
}
}
#endif
}
//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ROI<4F><49><EFBFBD>ڵĵ<DAB5><C4B5><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>淨<EFBFBD><E6B7A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>
SSG_planeCalibPara sg_getSieveBaseCalibPara(
SVzNL3DLaserLine* laser3DPoints,
int lineNum,
std::vector<SVzNL3DRangeD>& ROIs)
{
return sg_getPlaneCalibPara_ROIs(laser3DPoints, lineNum, ROIs);
}
Reference in New Issue Copy Permalink