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

1105 lines
35 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>
//<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);
cv::imwrite("distTransform.png", dtImage);
#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++)
{
for (int j = i + 1; j < filterSize; j++)
{
}
}
//<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));
wsImg.markers.resize(wsImg.height, std::vector<int>(wsImg.width, 1)); // <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͼΪ0
int maxValue = (int)maxVal + 2;
for (int i = 0; i < distTransform.rows; i++)
{
float* rowPtr = distTransform.ptr<float>(i);
for (int j = 0; j < distTransform.cols; j++)
{
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;
}
}
}
watershed(wsImg);
#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);
}
else { // <20><><EFBFBD>򣨸<EFBFBD><F2A3A8B8>ݱ<EFBFBD><DDB1><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD>ɲ<EFBFBD>ͬ<EFBFBD><CDAC>ɫ<EFBFBD><C9AB>
int color_r = (wsImg.markers[i][j] * 50) % 256;
int color_g = (color_r + 85) % 256;
int color_b = (color_r + 170) % 256;
waterShedResult.at<cv::Vec3b>(i, j) = cv::Vec3b(color_b, color_g, color_r);
}
}
}
cv::imwrite("watershed.png", waterShedResult);
#endif
#if 0
SSG_localPkParam searchWin;
searchWin.seachW_lines = (int)(algoParam.bagParam.bagW * 0.4);
searchWin.searchW_pts = (int)(algoParam.bagParam.bagW * 0.4);
std::vector<SSG_2DValueI> dt_peaks;
sg_getLocalPeaks_distTransform(distTransform, dt_peaks, searchWin);
//<2F><>ȡPeaks
int invlidDistToEdge = 50; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ե<EFBFBD><D4B5><EFBFBD><EFBFBD>Peak<61>ǷǷ<C7B7><C7B7>㡣
double minPeakValue = algoParam.bagParam.bagW / 8;
std::vector<SSG_2DValueI> peaks;
for (int i = 0; i < dt_peaks.size(); i++)
{
//<2F>߽紦<DFBD><E7B4A6>PeakΪ<6B><CEAA><EFBFBD>ϸ<EFBFBD>Peak<61><6B>ȥ<EFBFBD><C8A5>
int x_diff_0 = dt_peaks[i].x; //<2F><><EFBFBD><EFBFBD><EFBFBD>߾<EFBFBD><DFBE>룬<EFBFBD><EBA3AC>λΪmm<6D><6D><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߶<EFBFBD>Ϊ1mm<6D><6D>
int x_diff_1 = distTransform.cols - dt_peaks[i].x;//<2F><><EFBFBD>ұ߾<D2B1><DFBE><EFBFBD>
int y_diff_0 = dt_peaks[i].y;//<2F><><EFBFBD>ϱ߾<CFB1><DFBE><EFBFBD>
int y_diff_1 = distTransform.rows - dt_peaks[i].y;//<2F><><EFBFBD>±߾<C2B1><DFBE><EFBFBD>
if ((x_diff_0 < invlidDistToEdge) || (x_diff_1 < invlidDistToEdge) ||
(y_diff_0 < invlidDistToEdge) || (y_diff_1 < invlidDistToEdge) ||
(dt_peaks[i].valueD < minPeakValue))
continue;
//<2F><>distTranformIndexing<6E>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double pkValue = dt_peaks[i].valueD;
SSG_2DValueI a_peak = _backIndexingPeakPos(dt_peaks[i], distTranformIndexing);
a_peak.valueD = pkValue; // laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y].pt3D.z;
peaks.push_back(a_peak);
}
//<2F><><EFBFBD>ո߶<D5B8><DFB6><EFBFBD><EFBFBD><EFBFBD>
std::sort(peaks.begin(), peaks.end(), compareByHeight);
for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++)
featureMask.at<cv::Vec4i>(peaks[i].y, peaks[i].x)[3] = 1; //peak flag
#if 0
//ʹ<><CAB9><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5>zֵ<7A><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>任ֵ
for (int i = 0, i_max = peaks.size(); i < i_max; i++)
{
peaks[i].valueD = laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y].pt3D.z;
}
#endif
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD><DFB5><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>ӽ<EFBFBD><D3BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<SSG_peakRgnInfo> peakRgns;
int peakRgnId = 1;
for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++)
{
if (i == 3)
int kkk = 1;
SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]);
int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff;
if (pkRgnId > 0)
continue;
//<2F><><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD>ˮƽ<CBAE>ʹ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>õ<EFBFBD><C3B5>߽<EFBFBD><DFBD><EFBFBD>
std::vector< SSG_lineConotours> topContour;
std::vector< SSG_lineConotours> bottomContour;
std::vector< SSG_lineConotours> leftContour;
std::vector< SSG_lineConotours> rightContour;
int maxEdgeId_top = 0, maxEdgeId_btm = 0, maxEdgeId_left = 0, maxEdgeId_right = 0;
sg_peakXYScan(
laser3DPoints,
lineNum,
featureMask,
peaks[i],
algoParam.growParam,
algoParam.bagParam,
false,
topContour,
bottomContour,
leftContour,
rightContour,
&maxEdgeId_top,
&maxEdgeId_btm,
&maxEdgeId_left,
&maxEdgeId_right);
int vldSide = 0;
if (leftContour.size() > 30)
vldSide++;
if (rightContour.size() > 30)
vldSide++;
if (topContour.size() > 30)
vldSide++;
if (bottomContour.size() > 30)
vldSide++;
int invldSide = 0;
if (leftContour.size() < 10)
invldSide++;
if (rightContour.size() < 10)
invldSide++;
if (topContour.size() < 10)
invldSide++;
if (bottomContour.size() < 10)
invldSide++;
if ((vldSide < 3) || (invldSide > 0))
continue;
//ȫƥ<C8AB>䣺<EFBFBD><E4A3BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>϶<EFBFBD><CFB6>Ĵ<EFBFBD><C4B4>ӣ<EFBFBD><D3A3><EFBFBD>Ҫȫƥ<C8AB><C6A5>Ѱ<EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>ȷ<EFBFBD>ı<EFBFBD>
std::vector<SSG_edgeMatchInfo> matchTable_TB;
std::vector< SSG_matchPair> TB_pairs;
std::vector<SSG_conotourPair> TB_contourPairs;
int TB_matchNum = 0;
_getMatchTable(
topContour,
bottomContour,
maxEdgeId_top,
maxEdgeId_btm,
true, //isVScan,
vTreeStart,
hTreeStart,
matchTable_TB,
TB_pairs,
TB_contourPairs,
&TB_matchNum);
if (TB_matchNum < 25)
continue;
std::vector< SSG_matchPair> LR_pairs;
std::vector<SSG_edgeMatchInfo> matchTable_LR;
std::vector<SSG_conotourPair> LR_contourPairs;
int LR_matchNum = 0;
_getMatchTable(
leftContour,
rightContour,
maxEdgeId_left,
maxEdgeId_right,
false, //isHScan,
vTreeStart,
hTreeStart,
matchTable_LR,
LR_pairs,
LR_contourPairs,
&LR_matchNum);
if (LR_matchNum < 25)
continue;
int lowLevelChkFlag = 0;
SSG_peakRgnInfo a_pkRgn = _maxLikelihoodMatch(
laser3DPoints,
lineNum,
hvTreeSize,
peaks[i],
matchTable_TB,
TB_pairs,
TB_contourPairs,
TB_matchNum,
maxEdgeId_btm,
matchTable_LR,
LR_pairs,
LR_contourPairs,
LR_matchNum,
maxEdgeId_right,
allTreesInfo,
vTreeStart,
hTreeStart,
globalROI,
algoParam,
peakRgnId,
&lowLevelChkFlag);
if (a_pkRgn.pkRgnIdx > 0)
{
peakRgns.push_back(a_pkRgn);
peakRgnId++;
}
}
#if 1
///<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֤û<D6A4>д<EFBFBD><D0B4>ڴ<EFBFBD><DAB4>ӵ<EFBFBD>Ŀ<EFBFBD><C4BF>δ<EFBFBD><CEB4><EFBFBD><EFBFBD>
///<2F><><EFBFBD><EFBFBD>ʣ<EFBFBD>µ<EFBFBD>Ŀ<EFBFBD>꣬<EFBFBD><EAA3AC><EFBFBD><EFBFBD>û<EFBFBD>м<EFBFBD><D0BC><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD>
///ɨ<><C9A8>ʱ<EFBFBD><CAB1><EFBFBD>Ᵽ֤ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӳߴ磬Ȼ<E7A3AC><C8BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
while (1)
{
std::vector<SSG_peakRgnInfo> iter_objs;
//<2F><>û<EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD><EFBFBD>Peak<61>㱣<EFBFBD><E3B1A3>
std::vector<SSG_2DValueI> residualPeaks;
for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++)
{
SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]);
int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff;
if (pkRgnId == 0)
{
residualPeaks.push_back(peaks[i]);
}
}
if (residualPeaks.size() == 0)
break;
bool rgnPtAsEdge = true;
for (int ri = 0; ri < residualPeaks.size(); ri++)
{
SVzNL3DPosition* pk_pt = &(laser3DPoints[residualPeaks[ri].x].p3DPosition[residualPeaks[ri].y]);
int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff;
if (pkRgnId > 0)
continue;
//<2F><><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD>ˮƽ<CBAE>ʹ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>õ<EFBFBD><C3B5>߽<EFBFBD><DFBD><EFBFBD>
std::vector< SSG_lineConotours> resi_topContour;
std::vector< SSG_lineConotours> resi_bottomContour;
std::vector< SSG_lineConotours> resi_leftContour;
std::vector< SSG_lineConotours> resi_rightContour;
int resi_maxEdgeId_top = 0, resi_maxEdgeId_btm = 0, resi_maxEdgeId_left = 0, resi_maxEdgeId_right = 0;
sg_peakXYScan(
laser3DPoints,
lineNum,
featureMask,
residualPeaks[ri],
algoParam.growParam,
algoParam.bagParam,
true,
resi_topContour,
resi_bottomContour,
resi_leftContour,
resi_rightContour,
&resi_maxEdgeId_top,
&resi_maxEdgeId_btm,
&resi_maxEdgeId_left,
&resi_maxEdgeId_right);
if ((resi_topContour.size() == 0) || (resi_bottomContour.size() == 0) || (resi_leftContour.size() == 0) || (resi_rightContour.size() == 0))
continue;
//<2F>ֶμ<D6B6><CEBC><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD>Ⱥ<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>߶<EFBFBD><DFB6>Լ<EFBFBD><D4BC>ֶ<EFBFBD>ROI
//ȫƥ<C8AB>䣨<EFBFBD><E4A3A8>ˮƽ<CBAE><C6BD><EFBFBD>зֶε<D6B6><CEB5><EFBFBD><EFBFBD>п<EFBFBD><D0BF>ܾ<EFBFBD><DCBE><EFBFBD>
std::vector<SSG_edgeMatchInfo> matchTable_TB;
std::vector< SSG_matchPair> TB_pairs;
std::vector<SSG_conotourPair> TB_contourPairs;
int TB_matchNum = 0;
_getMatchTable(
resi_topContour,
resi_bottomContour,
resi_maxEdgeId_top,
resi_maxEdgeId_btm,
true, //isVScan,
vTreeStart,
hTreeStart,
matchTable_TB,
TB_pairs,
TB_contourPairs,
&TB_matchNum);
if (TB_matchNum < 25)
continue;
std::vector< SSG_matchPair> LR_pairs;
std::vector<SSG_edgeMatchInfo> matchTable_LR;
std::vector<SSG_conotourPair> LR_contourPairs;
int LR_matchNum = 0;
_getMatchTable(
resi_leftContour,
resi_rightContour,
resi_maxEdgeId_left,
resi_maxEdgeId_right,
false, //isHScan,
vTreeStart,
hTreeStart,
matchTable_LR,
LR_pairs,
LR_contourPairs,
&LR_matchNum);
if (LR_matchNum < 25)
continue;
int lowLevelChkFlag = 0;
SSG_peakRgnInfo a_pkRgn = _maxLikelihoodMatch(
laser3DPoints,
lineNum,
hvTreeSize,
peaks[ri],
matchTable_TB,
TB_pairs,
TB_contourPairs,
TB_matchNum,
resi_maxEdgeId_btm,
matchTable_LR,
LR_pairs,
LR_contourPairs,
LR_matchNum,
resi_maxEdgeId_right,
allTreesInfo,
vTreeStart,
hTreeStart,
globalROI,
algoParam,
peakRgnId,
&lowLevelChkFlag);
#if 0
if (lowLevelChkFlag > 0)
{
//lowLevelFlag_T + lowLevelFlag_B<<1 + lowLevelFlag_L<<2 + lowLevelFlag_R<<3;
if (lowLevelChkFlag & 0x01) //Top
{
}
if (lowLevelChkFlag & 0x02) //Bottom
{
}
if (lowLevelChkFlag & 0x04) //Left
{
}
if (lowLevelChkFlag & 0x08) //Rigjt
{
}
}
#endif
if (a_pkRgn.pkRgnIdx > 0)
{
iter_objs.push_back(a_pkRgn);
peakRgnId++;
}
}
if (iter_objs.size() == 0)
break;
peakRgns.insert(peakRgns.end(), iter_objs.begin(), iter_objs.end());
//Ϊ<><CEAA>һ<EFBFBD>ε<EFBFBD><CEB5><EFBFBD>׼<EFBFBD><D7BC>
peaks.clear();
peaks.insert(peaks.end(), residualPeaks.begin(), residualPeaks.end());
}
///<2F><>ʣ<EFBFBD><CAA3><EFBFBD><EFBFBD>СĿ<D0A1><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ײ<EFBFBD><D7B2><EFBFBD><EFBFBD>
///<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>δ֪<CEB4><D6AA>δ<EFBFBD><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Է<EFBFBD>ֹδ֪<CEB4><D6AA><EFBFBD><EFBFBD>ײ
///<2F><>¼<EFBFBD><C2BC><EFBFBD>еĴ<D0B5><C4B4><EFBFBD>1/4L*1/4W<34><57>Ŀ<EFBFBD><C4BF>
std::vector<SSG_2DValueI> smallObjPeaks; //<2F><>¼0.25L * 0.25W<EFBFBD><EFBFBD>Ŀ<EFBFBD>꣬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ײ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD><EFBFBD>Peak<61>㱣<EFBFBD><E3B1A3>
std::vector<SSG_2DValueI> residualPeaks;
for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++)
{
SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]);
int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff;
if (pkRgnId == 0)
{
residualPeaks.push_back(peaks[i]);
}
}
if (residualPeaks.size() > 0)
{
bool rgnPtAsEdge = true;
for (int ri = 0; ri < residualPeaks.size(); ri++)
{
//<2F><><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD>ˮƽ<CBAE>ʹ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>õ<EFBFBD><C3B5>߽<EFBFBD><DFBD><EFBFBD>
std::vector< SSG_lineConotours> resi_topContour;
std::vector< SSG_lineConotours> resi_bottomContour;
std::vector< SSG_lineConotours> resi_leftContour;
std::vector< SSG_lineConotours> resi_rightContour;
int resi_maxEdgeId_top = 0, resi_maxEdgeId_btm = 0, resi_maxEdgeId_left = 0, resi_maxEdgeId_right = 0;
sg_peakXYScan(
laser3DPoints,
lineNum,
featureMask,
residualPeaks[ri],
algoParam.growParam,
algoParam.bagParam,
true,
resi_topContour,
resi_bottomContour,
resi_leftContour,
resi_rightContour,
&resi_maxEdgeId_top,
&resi_maxEdgeId_btm,
&resi_maxEdgeId_left,
&resi_maxEdgeId_right);
//<2F><><EFBFBD><EFBFBD>ROI
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>޵<EFBFBD>
SSG_ROIRectD objROI = { 0, -1, 0, 0 };
for (int n = 0; n < resi_topContour.size(); n++)
{
std::vector<SSG_contourPtInfo>& a_line_contourPts = resi_topContour[n].contourPts;
for (int m = 0; m < a_line_contourPts.size(); m++)
{
if (objROI.right < objROI.left)
{
objROI.left = a_line_contourPts[m].edgePt.x;
objROI.right = a_line_contourPts[m].edgePt.x;
objROI.top = a_line_contourPts[m].edgePt.y;
objROI.bottom = a_line_contourPts[m].edgePt.y;
}
else
{
objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left;
objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right;
objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top;
objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom;
}
}
}
for (int n = 0; n < resi_bottomContour.size(); n++)
{
std::vector<SSG_contourPtInfo>& a_line_contourPts = resi_bottomContour[n].contourPts;
for (int m = 0; m < a_line_contourPts.size(); m++)
{
if (objROI.right < objROI.left)
{
objROI.left = a_line_contourPts[m].edgePt.x;
objROI.right = a_line_contourPts[m].edgePt.x;
objROI.top = a_line_contourPts[m].edgePt.y;
objROI.bottom = a_line_contourPts[m].edgePt.y;
}
else
{
objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left;
objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right;
objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top;
objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom;
}
}
}
for (int n = 0; n < resi_leftContour.size(); n++)
{
std::vector<SSG_contourPtInfo>& a_line_contourPts = resi_leftContour[n].contourPts;
for (int m = 0; m < a_line_contourPts.size(); m++)
{
if (objROI.right < objROI.left)
{
objROI.left = a_line_contourPts[m].edgePt.x;
objROI.right = a_line_contourPts[m].edgePt.x;
objROI.top = a_line_contourPts[m].edgePt.y;
objROI.bottom = a_line_contourPts[m].edgePt.y;
}
else
{
objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left;
objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right;
objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top;
objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom;
}
}
}
for (int n = 0; n < resi_rightContour.size(); n++)
{
std::vector<SSG_contourPtInfo>& a_line_contourPts = resi_rightContour[n].contourPts;
for (int m = 0; m < a_line_contourPts.size(); m++)
{
if (objROI.right < objROI.left)
{
objROI.left = a_line_contourPts[m].edgePt.x;
objROI.right = a_line_contourPts[m].edgePt.x;
objROI.top = a_line_contourPts[m].edgePt.y;
objROI.bottom = a_line_contourPts[m].edgePt.y;
}
else
{
objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left;
objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right;
objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top;
objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom;
}
}
}
//<2F><><EFBFBD><EFBFBD>ROI<4F><49>С
double obj_L = objROI.right - objROI.left;
double obj_W = objROI.bottom - objROI.top;
if (obj_L < obj_W)
{
double tmp_value = obj_W;
obj_W = obj_L;
obj_L = tmp_value;
}
if ((obj_L > algoParam.bagParam.bagL * 0.25) && (obj_W > algoParam.bagParam.bagW * 0.25))
{
smallObjPeaks.push_back(residualPeaks[ri]);
}
}
}
#endif
//Ŀ<><C4BF><EFBFBD><EFBFBD><EFBFBD>򣺷ֲ<F2A3BAB7> -> <20><><EFBFBD><EFBFBD><EFBFBD>߲<EFBFBD><DFB2><EFBFBD><EFBFBD><EFBFBD> -> <20><><EFBFBD><EFBFBD><EFBFBD>߲<EFBFBD><DFB2><EFBFBD>һ<EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (peakRgns.size() > 0)
{
double maxHeight = peakRgns[0].centerPos.z;
for (int i = 1; i < peakRgns.size(); i++)
{
if (maxHeight > peakRgns[i].centerPos.z)
maxHeight = peakRgns[i].centerPos.z;
}
//ȡͬ<C8A1>߶Ȳ<DFB6><C8B2><EFBFBD>Ŀ<EFBFBD><C4BF>
std::vector<SSG_peakRgnInfo> level0_objs;
for (int i = 0, i_max = (int)peakRgns.size(); i < i_max; i++)
{
double z_diff = peakRgns[i].centerPos.z - maxHeight;
if (z_diff < algoParam.bagParam.bagH / 2) //<2F>ֲ<EFBFBD>
{
level0_objs.push_back(peakRgns[i]);
}
}
peakRgns.clear();
peakRgns.insert(peakRgns.end(), level0_objs.begin(), level0_objs.end());
int level0_size = (int)peakRgns.size();
if (level0_size > 1) //<2F><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD>򣬷<EFBFBD><F2A3ACB7><EFBFBD>
{
//ȡY<C8A1><59>С<EFBFBD><D0A1>Ŀ<EFBFBD><C4BF>
double minY = 0;
double minY_idx = -1;
for (int i = 0; i < level0_size; i++)
{
if (minY_idx < 0)
{
minY = peakRgns[i].centerPos.y;
minY_idx = i;
}
else
{
if (minY > peakRgns[i].centerPos.y)
{
minY = peakRgns[i].centerPos.y;
minY_idx = i;
}
}
}
std::vector<int> row_0_outlier;
for (int i = 0; i < level0_size; i++)
{
double y_diff = peakRgns[i].centerPos.y - minY;
if (y_diff < algoParam.bagParam.bagW / 2) //<2F><>һ<EFBFBD><D2BB>
objOps.push_back(peakRgns[i]);
else
row_0_outlier.push_back(i); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD>ż<EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD>
}
//<2F>Ե<EFBFBD>һ<EFBFBD>е<EFBFBD>Ŀ<EFBFBD>갴<EFBFBD><EAB0B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (objOps.size() > 1)
{
std::sort(objOps.begin(), objOps.end(), compareByXValue);
}
for (int i = 0; i < row_0_outlier.size(); i++)
objOps.push_back(peakRgns[row_0_outlier[i]]);
#if 0
for (int i = level0_end + 1; i < peakRgns.size(); i++)
objOps.push_back(peakRgns[i]);
#endif
}
else
objOps.insert(objOps.end(), peakRgns.begin(), peakRgns.end());
}
//<2F><>ײ<EFBFBD><D7B2><EFBFBD><EFBFBD>
if ((objOps.size() > 0) && (smallObjPeaks.size() > 0))
{
SSG_peakRgnInfo* highest_obj = &objOps[0];
double objZ = highest_obj->centerPos.z;
for (int i = 0; i < smallObjPeaks.size(); i++)
{
SSG_2DValueI* a_samllPk = &smallObjPeaks[i];
if (highest_obj->centerPos.z > a_samllPk->valueD + algoParam.bagParam.bagH / 2)
{
SVzNL3DPosition* smallPkPt = &laser3DPoints[a_samllPk->x].p3DPosition[a_samllPk->y];
double dist = sqrt(pow(highest_obj->centerPos.x - smallPkPt->pt3D.x, 2) + pow(highest_obj->centerPos.y - smallPkPt->pt3D.y, 2));
double dia_angle = sqrt(pow(highest_obj->objSize.dWidth, 2) + pow(highest_obj->objSize.dHeight, 2));
//ͬ<><CDAC><EFBFBD>Ƚ<EFBFBD>
double z_diff = smallPkPt->pt3D.z - objZ;
if (z_diff < algoParam.bagParam.bagH / 2) //<2F>ֲ<EFBFBD>
{
if (dist < dia_angle / 2)
objOps.clear(); //<2F><><EFBFBD>μ<EFBFBD><CEBC><EFBFBD><EFBFBD><EFBFBD>Ч
}
}
}
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ͷ<EFBFBD><CDB6><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>۱궨<DBB1><EAB6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ
for (int i = 0; i < lineNum; i++)
sg_lineDataR(&laser3DPoints[i], poseCalibPara.invRMatrix, -1);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ͷ<EFBFBD><CDB6><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ
double invMatrix[3][3];
invMatrix[0][0] = poseCalibPara.invRMatrix[0];
invMatrix[0][1] = poseCalibPara.invRMatrix[1];
invMatrix[0][2] = poseCalibPara.invRMatrix[2];
invMatrix[1][0] = poseCalibPara.invRMatrix[3];
invMatrix[1][1] = poseCalibPara.invRMatrix[4];
invMatrix[1][2] = poseCalibPara.invRMatrix[5];
invMatrix[2][0] = poseCalibPara.invRMatrix[6];
invMatrix[2][1] = poseCalibPara.invRMatrix[7];
invMatrix[2][2] = poseCalibPara.invRMatrix[8];
for (int i = 0, i_max = (int)objOps.size(); i < i_max; i++)
{
SSG_EulerAngles euAngle = { objOps[i].centerPos.x_roll, objOps[i].centerPos.y_pitch, objOps[i].centerPos.z_yaw };
double pose[3][3];
eulerToRotationMatrixZYX(euAngle, pose);
double resultMatrix[3][3];
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
resultMatrix[i][j] = 0;
for (int m = 0; m < 3; m++)
resultMatrix[i][j] += invMatrix[i][m] * pose[m][j];
}
}
SSG_EulerAngles resultEuAngle = rotationMatrixToEulerZYX(resultMatrix);
objOps[i].centerPos.z_yaw = resultEuAngle.yaw;
double x = objOps[i].centerPos.x * poseCalibPara.invRMatrix[0] +
objOps[i].centerPos.y * poseCalibPara.invRMatrix[1] +
objOps[i].centerPos.z * poseCalibPara.invRMatrix[2];
double y = objOps[i].centerPos.x * poseCalibPara.invRMatrix[3] +
objOps[i].centerPos.y * poseCalibPara.invRMatrix[4] +
objOps[i].centerPos.z * poseCalibPara.invRMatrix[5];
double z = objOps[i].centerPos.x * poseCalibPara.invRMatrix[6] +
objOps[i].centerPos.y * poseCalibPara.invRMatrix[7] +
objOps[i].centerPos.z * poseCalibPara.invRMatrix[8];
objOps[i].centerPos.x = x;
objOps[i].centerPos.y = y;
objOps[i].centerPos.z = z;
}
#endif
//ˮƽ<CBAE>ʹ<EFBFBD>ֱ<EFBFBD><D6B1>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>
//<2F><><EFBFBD>о<EFBFBD><D0BE><EFBFBD><EFBFBD>任
//<2F><>ˮ<EFBFBD><CBAE><EFBFBD>ָ<EFBFBD>
//<2F><>z<EFBFBD>߶ȼ<DFB6><C8BC><EFBFBD>Ŀ<EFBFBD><C4BF>
}
Reference in New Issue Copy Permalink