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

616 lines
20 KiB
C++
Raw Normal View History

更新工件算法
2025-11-10 22:44:31 +08:00
#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "WD_particleSizeMeasure_Export.h"
#include <opencv2/opencv.hpp>
#include <limits>
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
std::string m_strVersion = "1.0.0";
const char* wd_particleSegVersion(void)
{
return m_strVersion.c_str();
}
更新工件算法
2025-11-10 22:44:31 +08:00
//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>п<EFBFBD><D0BF><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>Ͳο<CDB2><CEBF><EFBFBD>ƽƽ<C6BD>棬<EFBFBD><E6A3AC><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><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 wd_getBaseCalibPara(
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
return sg_getPlaneCalibPara2(scanLines);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void wd_lineDataR(
std::vector< SVzNL3DPosition>& a_line,
const double* camPoseR,
double groundH)
{
lineDataRT_vector(a_line, camPoseR, groundH);
}
void wd_noiseFilter(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SWD_PSM_algoParam algoParam,
int* errCode)
{
*errCode = 0;
int lineNum = (int)scanLines.size();
int nPointCnt = (int)scanLines[0].size();
bool vldGrid = true;
for (int i = 0; i < lineNum; i++)
{
if (nPointCnt != (int)scanLines[i].size())
vldGrid = false;
wd_vectorDataRemoveOutlier_overwrite(
scanLines[i],
algoParam.filterParam);
}
if (false == vldGrid)
{
*errCode = SG_ERR_3D_DATA_INVLD;
return;
}
//ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int hLineNum = nPointCnt; //Grid<69><64>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>ߵĵ<DFB5><C4B5><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD>ˮƽɨ<C6BD><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<std::vector<SVzNL3DPosition>> filterHLines;
filterHLines.resize(hLineNum);
for (int i = 0; i < hLineNum; i++)
filterHLines[i].resize(lineNum);
for (int line = 0; line < lineNum; line++)
{
for (int j = 0; j < hLineNum; j++)
{
filterHLines[j][line] = scanLines[line][j];
filterHLines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
filterHLines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
}
}
for (int hLine = 0; hLine < hLineNum; hLine++)
{
//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
std::vector<SVzNL3DPosition> filterData;
std::vector<int> lineNoise;
sg_lineDataRemoveOutlier(
(SVzNL3DPosition*)filterHLines[hLine].data(),
(int)filterHLines[hLine].size(),
algoParam.filterParam,
filterData,
lineNoise);
for (int j = 0; j < lineNoise.size(); j++)
{
int lineIdx = lineNoise[j];
scanLines[lineIdx][hLine].pt3D.z = 0;
}
}
return;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void wd_particleSizeMeasure(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SWD_paricleSizeParam particleSizeParam,
const SSG_planeCalibPara groundCalibPara,
const SWD_PSM_algoParam algoParam,
std::vector<SWD_ParticlePosInfo>& particles,
int* errCode)
{
int lineNum = (int)scanLines.size();
if (lineNum == 0)
{
*errCode = SG_ERR_3D_DATA_NULL;
return;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
wd_noiseFilter(scanLines, algoParam, errCode);
if (*errCode != 0)
return;
///<2F><>ʼ<EFBFBD><CABC><EFBFBD>ݴ<EFBFBD><DDB4><EFBFBD>
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>䡢<EFBFBD><E4A1A2><EFBFBD><EFBFBD>z<EFBFBD><7A>ֵ<EFBFBD><D6B5>V<EFBFBD><56>L<EFBFBD><4C>
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD>ݴ<EFBFBD><DDB4><EFBFBD>
std::vector<SSG_lineFeature> all_vLineFeatures;
for (int i = 0; i < lineNum; i++)
{
if (i == 202)
int k = 1;
SSG_lineFeature a_line_features;
a_line_features.lineIdx = i;
wd_getLineCornerFeature_PSM(
&scanLines[i][0],
(int)scanLines[i].size(),
i,
groundCalibPara.planeHeight,
algoParam.cornerParam,
&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>
}
//<2F><><EFBFBD><EFBFBD>ˮƽɨ<C6BD><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int nPointCnt = (int)scanLines[0].size();
std::vector<std::vector<SVzNL3DPosition>> hLines;
hLines.resize(nPointCnt);
for (int i = 0; i < nPointCnt; i++)
hLines[i].resize(lineNum);
for (int line = 0; line < lineNum; line++)
{
for (int j = 0; j < nPointCnt; j++)
{
scanLines[line][j].nPointIdx = 0;;
hLines[j][line] = scanLines[line][j];
hLines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
hLines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
}
}
std::vector<SSG_lineFeature> all_hLineFeatures;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>
for (int hLine = 0; hLine < nPointCnt; hLine++)
{
if (hLine == 14)
int kkk = 1;
SSG_lineFeature a_hLine_featrues;
a_hLine_featrues.lineIdx = hLine;
wd_getLineCornerFeature_PSM(
&hLines[hLine][0],
(int)hLines[hLine].size(),
hLine,
groundCalibPara.planeHeight,
algoParam.cornerParam,
&a_hLine_featrues);
//if ((a_hLine_featrues.features.size() > 0) || (a_hLine_featrues.endings.size() > 0))
all_hLineFeatures.push_back(a_hLine_featrues);//<2F><><EFBFBD><EFBFBD>Ҳ<EFBFBD><D2B2><EFBFBD>룬<EFBFBD><EBA3AC>֤<EFBFBD>ܰ<EFBFBD><DCB0>к<EFBFBD><D0BA><EFBFBD><EFBFBD><EFBFBD>
}
/// ͳ<><CDB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ұ<EFBFBD><D2B0>С
SWD_pointCloudPara pntCloudPara = wd_getPointCloudPara(scanLines);
SVzNLRangeD x_range = pntCloudPara.xRange;
SVzNLRangeD y_range = pntCloudPara.yRange;
SSG_ROIRectD globalROI;
globalROI.left = pntCloudPara.xRange.min;
globalROI.right = pntCloudPara.xRange.max;
globalROI.top = pntCloudPara.yRange.min;
globalROI.bottom = pntCloudPara.yRange.max;
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD>跽<EFBFBD><E8B7BD><EFBFBD><EFBFBD>
std::vector<SSG_featureTree> v_feature_trees;
std::vector<SSG_featureTree> v_ending_trees;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
wd_getFeatureGrowingTrees_noTypeMatch(
all_vLineFeatures,
v_feature_trees,
v_ending_trees,
algoParam.growParam);
//ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<SSG_featureTree> h_feature_trees;
std::vector<SSG_featureTree> h_ending_trees;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
wd_getFeatureGrowingTrees_noTypeMatch(
all_hLineFeatures,
h_feature_trees,
h_ending_trees,
algoParam.growParam);
//<2F><><EFBFBD><EFBFBD>Mask
double scale;
if ((pntCloudPara.scale_x < 0) || (pntCloudPara.scale_y < 0))
{
*errCode = SG_ERR_3D_DATA_INVLD;
return;
}
if(pntCloudPara.scale_x < pntCloudPara.scale_y)
scale = pntCloudPara.scale_x;
else
scale = pntCloudPara.scale_y;
//<2F><><EFBFBD><EFBFBD><EFBFBD>任Mask<73><6B><EFBFBD><EFBFBD>1mmΪ<6D><CEAA><EFBFBD><EFBFBD><EFBFBD>߶<EFBFBD>
double inerPolateDistTh = scale * 10; //<2F><>ֵ<EFBFBD><D6B5><EFBFBD>ޣ<EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>ֵ
int edgeSkip = 2;
int maskX = (int)(x_range.max - x_range.min)/scale + 1;
int maskY = (int)(y_range.max - y_range.min)/scale + 1;
if ((maskX < 16) || (maskY < 16))
return;
maskY = maskY + edgeSkip * 2;
maskX = maskX + edgeSkip * 2;
cv::Mat distTranformMask(maskY, maskX, CV_32FC1, 0.0f); //<2F><><EFBFBD><EFBFBD><EFBFBD>任Mask<73><6B><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>Ϊһ<CEAA><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ1e+6
cv::Mat distTranformIndexing(maskY, maskX, CV_32SC2, cv::Vec2i(0, 0)); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
cv::Mat featureMask = cv::Mat::zeros(nPointCnt, lineNum, CV_32SC4);
pointClout2DProjection(
scanLines,
x_range,
y_range,
scale,
edgeSkip,
inerPolateDistTh, //<2F><>ֵ<EFBFBD><D6B5>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD>ֵ<EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
distTranformMask,//ͶӰ<CDB6><D3B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD>ʼ<EFBFBD><CABC>Ϊһ<CEAA><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ1e+6
distTranformIndexing //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڻ<EFBFBD><DABB><EFBFBD>3D<33><44><EFBFBD><EFBFBD>
);
std::vector<SSG_treeInfo> allTreesInfo; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߽<EFBFBD>
//<2F><>ע:<3A><>ֱ
SSG_treeInfo a_nullTree;
memset(&a_nullTree, 0, sizeof(SSG_treeInfo));
allTreesInfo.push_back(a_nullTree); //<2F><><EFBFBD>ִ洢λ<E6B4A2><CEBB><EFBFBD><EFBFBD>treeIdx<64><78>ͬλ<CDAC>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><>ע<EFBFBD><D7A2>ֱ<EFBFBD>߽<EFBFBD>
int treeID = 1;
for (int i = 0, i_max = (int)v_ending_trees.size(); i < i_max; i++)
{
SSG_featureTree* a_vEdgeTree = &v_ending_trees[i];
//<2F><>¼Tree<65><65><EFBFBD><EFBFBD>Ϣ
SSG_treeInfo a_treeInfo;
a_treeInfo.vTreeFlag = 1;
a_treeInfo.treeIdx = treeID;
a_treeInfo.treeType = a_vEdgeTree->treeType;
a_treeInfo.sLineIdx = a_vEdgeTree->sLineIdx;
a_treeInfo.eLineIdx = a_vEdgeTree->eLineIdx;
a_treeInfo.roi = a_vEdgeTree->roi;
allTreesInfo.push_back(a_treeInfo);
//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
for (int j = 0, j_max = (int)a_vEdgeTree->treeNodes.size(); j < j_max; j++)
{
SSG_basicFeature1D* a_feature = &a_vEdgeTree->treeNodes[j];
if (scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.z > 1e-4)//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>˺<EFBFBD><CBBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0
{
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType;
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx &= 0xffff;
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx += treeID << 16;
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[0] = treeID; //edgeID
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vEdgeTree->treeType;
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[2] = 1; //vscan
int px = (int)((scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.x - x_range.min)/scale) + edgeSkip;
int py = (int)((scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.y - y_range.min)/scale) + edgeSkip;
distTranformMask.at<float>(py, px) = 0;
}
}
treeID++;
}
//<2F><>עˮƽ<CBAE>߽<EFBFBD>
for (int i = 0, i_max = (int)h_ending_trees.size(); i < i_max; i++)
{
SSG_featureTree* a_hEdgeTree = &h_ending_trees[i];
//<2F><>¼Tree<65><65><EFBFBD><EFBFBD>Ϣ
SSG_treeInfo a_treeInfo;
a_treeInfo.vTreeFlag = 0;
a_treeInfo.treeIdx = treeID;
a_treeInfo.treeType = a_hEdgeTree->treeType;
a_treeInfo.sLineIdx = a_hEdgeTree->sLineIdx;
a_treeInfo.eLineIdx = a_hEdgeTree->eLineIdx;
a_treeInfo.roi.left = a_hEdgeTree->roi.top; //ˮƽɨ<C6BD><C9A8>xy<78>ǽ<EFBFBD><C7BD><EFBFBD><EFBFBD><EFBFBD>
a_treeInfo.roi.right = a_hEdgeTree->roi.bottom;
a_treeInfo.roi.top = a_hEdgeTree->roi.left;
a_treeInfo.roi.bottom = a_hEdgeTree->roi.right;
allTreesInfo.push_back(a_treeInfo);
//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
for (int j = 0, j_max = (int)a_hEdgeTree->treeNodes.size(); j < j_max; j++)
{
SSG_basicFeature1D* a_feature = &a_hEdgeTree->treeNodes[j];
if (scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.z > 1e-4)//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>˺<EFBFBD><CBBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0
{
int existEdgeId = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx >> 16;
if (existEdgeId == 0)
{
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4;
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx &= 0xffff;
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += treeID << 16;
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[0] = treeID;
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hEdgeTree->treeType << 4;
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[2] = 2;//hsan flag
int px = (int)((scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.x - x_range.min)/scale) + edgeSkip;
int py = (int)((scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.y - y_range.min)/scale) + edgeSkip;
distTranformMask.at<float>(py, px) = 0;
}
}
}
treeID++;
}
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע
int hvTreeIdx = treeID;
int vTreeStart = treeID;
for (int i = 0, i_max = (int)v_feature_trees.size(); i < i_max; i++)
{
SSG_featureTree* a_vTree = &v_feature_trees[i];
//<2F><>¼Tree<65><65><EFBFBD><EFBFBD>Ϣ
SSG_treeInfo a_treeInfo;
a_treeInfo.vTreeFlag = 1;
a_treeInfo.treeIdx = hvTreeIdx;
a_treeInfo.treeType = a_vTree->treeType;
a_treeInfo.sLineIdx = a_vTree->sLineIdx;
a_treeInfo.eLineIdx = a_vTree->eLineIdx;
a_treeInfo.roi = a_vTree->roi;
allTreesInfo.push_back(a_treeInfo);
//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++)
{
SSG_basicFeature1D* a_feature = &a_vTree->treeNodes[j];
if (scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.z > 1e-4)//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>˺<EFBFBD><CBBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0
{
int existEdgeId = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx >> 16;
if (existEdgeId == 0)
{
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType;
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx &= 0xffff;
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx += hvTreeIdx << 16;
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[0] = hvTreeIdx; //edgeID
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vTree->treeType;
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[2] = 1; //vscan
int px = (int)((scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.x - x_range.min)/scale) + edgeSkip;
int py = (int)((scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.y - y_range.min)/scale) + edgeSkip;
distTranformMask.at<float>(py, px) = 0;
}
}
}
hvTreeIdx++;
}
int hTreeStart = hvTreeIdx;
//<2F><>ע:ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD>
for (int i = 0, i_max = (int)h_feature_trees.size(); i < i_max; i++)
{
SSG_featureTree* a_hTree = &h_feature_trees[i];
//<2F><>¼Tree<65><65><EFBFBD><EFBFBD>Ϣ
SSG_treeInfo a_treeInfo;
a_treeInfo.vTreeFlag = 0;
a_treeInfo.treeIdx = hvTreeIdx;
a_treeInfo.treeType = a_hTree->treeType;
a_treeInfo.sLineIdx = a_hTree->sLineIdx;
a_treeInfo.eLineIdx = a_hTree->eLineIdx;
a_treeInfo.roi.left = a_hTree->roi.top; //ˮƽɨ<C6BD><C9A8>xy<78>ǽ<EFBFBD><C7BD><EFBFBD><EFBFBD><EFBFBD>
a_treeInfo.roi.right = a_hTree->roi.bottom;
a_treeInfo.roi.top = a_hTree->roi.left;
a_treeInfo.roi.bottom = a_hTree->roi.right;
allTreesInfo.push_back(a_treeInfo);
//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++)
{
SSG_basicFeature1D* a_feature = &a_hTree->treeNodes[j];
if (scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.z > 1e-4)//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>˺<EFBFBD><CBBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0
{
int existEdgeId = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx >> 16;
if (existEdgeId == 0)
{
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4;
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx &= 0xffff;
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += hvTreeIdx << 16;
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[0] = hvTreeIdx;
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hTree->treeType << 4;
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[2] = 2;//hsan flag
int px = (int)((scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.x - x_range.min)/scale) + edgeSkip;
int py = (int)((scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.y - y_range.min)/scale) + edgeSkip;
distTranformMask.at<float>(py, px) = 0;
}
}
}
hvTreeIdx++;
}
int hvTreeSize = hvTreeIdx;
double x_scale = pntCloudPara.scale_x;
double y_scale = pntCloudPara.scale_y;
//<2F><><EFBFBD>о<EFBFBD><D0BE><EFBFBD><EFBFBD>任<EFBFBD><E4BBBB>Ȼ<EFBFBD><C8BB>ʹ<EFBFBD>÷<EFBFBD>ˮ<EFBFBD><CBAE><EFBFBD>㷨<EFBFBD><E3B7A8><EFBFBD>зָ<D0B7>
cv::Mat distTransform;
sg_distanceTrans(distTranformMask, distTransform, 0);
#if OUTPUT_DEBUG //debug
cv::Mat maskImage;
cv::normalize(distTranformMask, maskImage, 0, 255, cv::NORM_MINMAX, CV_8U);
cv::imwrite("distTransformMask.png", maskImage);
cv::Mat dtImage;
cv::normalize(distTransform, dtImage, 0, 255, cv::NORM_MINMAX, CV_8U);
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
cv::Mat dtImage_color;
cv::cvtColor(dtImage, dtImage_color, cv::COLOR_GRAY2BGR);
cv::imwrite("distTransform.png", dtImage_color);
更新工件算法
2025-11-10 22:44:31 +08:00
#endif
//Ѱ<><D1B0>Peak<61><6B>Peakȷ<6B><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PeakΪ<6B><CEAA><EFBFBD>ӵ<EFBFBD><D3B5><EFBFBD><EFBFBD>з<EFBFBD>ˮ<EFBFBD>뷽<EFBFBD><EBB7BD><EFBFBD>ָ<EFBFBD>
double minW = particleSizeParam.minSize.width;
SSG_localPkParam searchWin;
searchWin.seachW_lines = (int)(minW * 0.4/scale);
searchWin.searchW_pts = (int)(minW * 0.4/scale);
std::vector<SSG_2DValueI> dt_peaks;
sg_getLocalPeaks_distTransform(distTransform, dt_peaks, searchWin);
//<2F>Դ<EFBFBD>С<EFBFBD><D0A1><EFBFBD>й<EFBFBD><D0B9><EFBFBD>
double minDistTh = minW * 0.4 / scale;
std::vector<SSG_2DValueI> filter_dt_peaks;
for (int i = 0, i_max = (int)dt_peaks.size(); i < i_max; i++)
{
if (dt_peaks[i].valueD > minDistTh)
filter_dt_peaks.push_back(dt_peaks[i]);
}
//<2F>Ծ<EFBFBD><D4BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>й<EFBFBD><D0B9>ˣ<EFBFBD><CBA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>任<EFBFBD>൱<EFBFBD><E0B5B1><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD>Բ<EFBFBD><D4B2><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD><EFBFBD><EFBFBD>Բһ<D4B2><D2BB><EFBFBD><EFBFBD><EFBFBD>ཻ<EFBFBD><E0BDBB><EFBFBD><EFBFBD><EFBFBD>ˣ<EFBFBD>R1+R2 < Բ<>ľ<EFBFBD><C4BE><EFBFBD>
int filterSize = (int)filter_dt_peaks.size();
for (int i = 0; i < filterSize; i++)
{
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
SSG_2DValueI& obj_0 = filter_dt_peaks[i];
更新工件算法
2025-11-10 22:44:31 +08:00
for (int j = i + 1; j < filterSize; j++)
{
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
SSG_2DValueI& obj_1 = filter_dt_peaks[j];
double dist = sqrt(pow(obj_0.x - obj_1.x, 2) + pow(obj_0.y - obj_1.y, 2));
double distTh = dist * 1.2;
if ((obj_0.valueD + obj_1.valueD) > distTh) //<2F>ϲ<EFBFBD>
{
if (obj_0.valueD < obj_1.valueD)
obj_0.value = -1;
else
obj_1.value = -1;
}
更新工件算法
2025-11-10 22:44:31 +08:00
}
}
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
//<2F><>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
std::vector<SSG_2DValueI> vld_dt_peaks;
for (int i = 0; i < filterSize; i++)
{
if (filter_dt_peaks[i].value < 0)
continue;
vld_dt_peaks.push_back(filter_dt_peaks[i]);
}
#if OUTPUT_DEBUG //debug
int dbg_seedNum = (int)vld_dt_peaks.size();
for (int i = 0; i < dbg_seedNum; i++)
{
int dbg_px = vld_dt_peaks[i].x;
int dbg_py = vld_dt_peaks[i].y;
//dtImage_color.at<cv::Vec3b>(dbg_py, dbg_px) = cv::Vec3b(0, 0, 255);
cv::circle(dtImage_color, cv::Point(dbg_px, dbg_py), 3, cv::Scalar(0, 0, 255), -1);
}
cv::imwrite("distTransform_seed.png", dtImage_color);
#endif
更新工件算法
2025-11-10 22:44:31 +08:00
//<2F><>ˮ<EFBFBD><CBAE><EFBFBD>ָ<EFBFBD>
//ɨ<><C9A8><EFBFBD>߽磬<DFBD><E7A3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>߽缯<DFBD><E7BCAF>
//ͨ<><CDA8>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>ڱ߽<DAB1><DFBD>ж<EFBFBD><D0B6>Ƿ<EFBFBD><C7B7>ϲ<EFBFBD><CFB2><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сֵ
double minVal, maxVal;
cv::Point minLoc, maxLoc;
// <20><><EFBFBD><EFBFBD>minMaxLoc<6F><63><EFBFBD><EFBFBD>
cv::minMaxLoc(distTransform, &minVal, &maxVal, &minLoc, &maxLoc);
//<2F><>ˮ<EFBFBD><CBAE><EFBFBD>㷨<EFBFBD><E3B7A8><EFBFBD>зָ<D0B7>
SWD_waterShedImage wsImg;
wsImg.width = distTransform.cols;
wsImg.height = distTransform.rows;
wsImg.gray.resize(wsImg.height, std::vector<int>(wsImg.width));
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
wsImg.markers.resize(wsImg.height, std::vector<int>(wsImg.width, 1)); // <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͼΪ1,<2C><><EFBFBD><EFBFBD>
更新工件算法
2025-11-10 22:44:31 +08:00
int maxValue = (int)maxVal + 2;
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
int maxLevel = (int)(maxVal - minVal);
更新工件算法
2025-11-10 22:44:31 +08:00
for (int i = 0; i < distTransform.rows; i++)
{
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
if (i == 758)
int kkk = 1;
更新工件算法
2025-11-10 22:44:31 +08:00
float* rowPtr = distTransform.ptr<float>(i);
for (int j = 0; j < distTransform.cols; j++)
{
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
if (j == 171)
int kkk = 1;
更新工件算法
2025-11-10 22:44:31 +08:00
float disValue = rowPtr[j];
if (disValue < 1e-4) //<2F>߽<EFBFBD><DFBD>ͱ<EFBFBD><CDB1><EFBFBD>
wsImg.gray[i][j] = maxValue;
else
{
wsImg.gray[i][j] = (int)(maxVal - disValue);
wsImg.markers[i][j] = 0;
}
}
}
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
int startMarkerID = 2;
wd_seedWatershed(wsImg, vld_dt_peaks, maxLevel, startMarkerID);
更新工件算法
2025-11-10 22:44:31 +08:00
#if OUTPUT_DEBUG //debug
cv::Mat waterShedResult(wsImg.height, wsImg.width, CV_8UC3);
for (int i = 0; i < wsImg.height; ++i) {
for (int j = 0; j < wsImg.width; ++j) {
if (wsImg.markers[i][j] == -1) { // <20><>ˮ<EFBFBD><CBAE><EFBFBD>߽磨<DFBD><E7A3A8>ɫ<EFBFBD><C9AB>
waterShedResult.at<cv::Vec3b>(i, j) = cv::Vec3b(0,0,255);
}
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
else if (wsImg.markers[i][j] < 2)
{
waterShedResult.at<cv::Vec3b>(i, j) = cv::Vec3b(200, 200, 200);
}
else
{ // <20><><EFBFBD>򣨸<EFBFBD><F2A3A8B8>ݱ<EFBFBD><DDB1><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD>ɲ<EFBFBD>ͬ<EFBFBD><CDAC>ɫ<EFBFBD><C9AB>
int color_r = (wsImg.markers[i][j] * 97) % 256;
int color_g = (wsImg.markers[i][j] * 73) % 256;
int color_b = (wsImg.markers[i][j] * 59) % 256;
更新工件算法
2025-11-10 22:44:31 +08:00
waterShedResult.at<cv::Vec3b>(i, j) = cv::Vec3b(color_b, color_g, color_r);
}
}
}
cv::imwrite("watershed.png", waterShedResult);
#endif
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
//<2F><><EFBFBD>ɷָ<C9B7><D6B8><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>
//<2F><><EFBFBD><EFBFBD>ÿ<EFBFBD><C3BF>3D<33><44><EFBFBD><EFBFBD>ͶӰ<CDB6>ϵ<EFBFBD>λ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD>3D<33><44>
std::vector<std::vector< SVzNL3DPoint>> segObjs;
int maxMkID = startMarkerID + (int)vld_dt_peaks.size();
segObjs.resize(maxMkID);
for (int line = 0; line < lineNum; line++)
{
for (int i = 0; i < nPointCnt; i++)
更新工件算法
2025-11-10 22:44:31 +08:00
{
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
SVzNL3DPosition* pt3D = &scanLines[line][i];
pt3D->nPointIdx = 0;
if (pt3D->pt3D.z < 1e-4)
更新工件算法
2025-11-10 22:44:31 +08:00
continue;
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
double x = pt3D->pt3D.x;
double y = pt3D->pt3D.y;
int px = (int)(x - x_range.min) / scale + edgeSkip;
int py = (int)(y - y_range.min) / scale + edgeSkip;
更新工件算法
2025-11-10 22:44:31 +08:00
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
int marker = wsImg.markers[py][px];
if ((marker >= startMarkerID)&&( marker <= maxMkID))
更新工件算法
2025-11-10 22:44:31 +08:00
{
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
pt3D->nPointIdx = marker;
segObjs[marker].push_back(pt3D->pt3D);
更新工件算法
2025-11-10 22:44:31 +08:00
}
}
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
}
更新工件算法
2025-11-10 22:44:31 +08:00
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>
for (int i = startMarkerID; i < maxMkID; i++)
{
if (segObjs[i].size() < 10)
continue;
//<2F><>ȡͶӰ,<2C><>ͳ<EFBFBD><CDB3><EFBFBD><EFBFBD>СZ<D0A1><5A><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Z
double minZ = -1;
double maxZ = 0;
std::vector<cv::Point2f> points;
int ptSize = (int)segObjs[i].size();
for (int m = 0; m < ptSize; m++)
更新工件算法
2025-11-10 22:44:31 +08:00
{
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
float x = (float)segObjs[i][m].x;
float y = (float)segObjs[i][m].y;
points.push_back(cv::Point2f(x, y));
if (minZ < 0)
更新工件算法
2025-11-10 22:44:31 +08:00
{
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
minZ = segObjs[i][m].z;
maxZ = segObjs[i][m].z;
更新工件算法
2025-11-10 22:44:31 +08:00
}
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
else
更新工件算法
2025-11-10 22:44:31 +08:00
{
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
if (minZ > segObjs[i][m].z) minZ = segObjs[i][m].z;
if (maxZ < segObjs[i][m].z) maxZ = segObjs[i][m].z;
更新工件算法
2025-11-10 22:44:31 +08:00
}
}
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
if (points.size() == 0)
continue;
//<2F><>С<EFBFBD><D0A1><EFBFBD>Ӿ<EFBFBD>
// <20><>С<EFBFBD><D0A1><EFBFBD>Ӿ<EFBFBD><D3BE><EFBFBD>
cv::RotatedRect rect = minAreaRect(points);
cv::Point2f vertices[4];
rect.points(vertices);
double width = rect.size.width; //ͶӰ<CDB6>Ŀ<EFBFBD><C4BF>͸ߣ<CDB8> <20><>Ӧ<EFBFBD><D3A6><EFBFBD>ӵij<D3B5><C4B3>Ϳ<EFBFBD>
double height = rect.size.height;
if (width < height)
更新工件算法
2025-11-10 22:44:31 +08:00
{
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
double tmp = height;
height = width;
width = tmp;
更新工件算法
2025-11-10 22:44:31 +08:00
}
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
SWD_ParticlePosInfo a_obj;
a_obj.size.length = width;
a_obj.size.width = height;
a_obj.size.height = maxZ - minZ;
for (int m = 0; m < 4; m++)
更新工件算法
2025-11-10 22:44:31 +08:00
{
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
SVzNL3DPoint vPt_btm = { vertices[m].x, vertices[m].y, maxZ };
SVzNL3DPoint vPt_top = { vertices[m].x, vertices[m].y, minZ };
a_obj.vertix[m] = vPt_btm;
a_obj.vertix[m + 4] = vPt_top;
更新工件算法
2025-11-10 22:44:31 +08:00
}
合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本
2025-11-10 23:24:06 +08:00
particles.push_back(a_obj);
}
更新工件算法
2025-11-10 22:44:31 +08:00
}
Reference in New Issue Copy Permalink