377 lines
10 KiB
C++
377 lines
10 KiB
C++
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include <vector>
|
||
|
||
void _updateROI(SSG_ROIRectD& roi, SVzNL3DPoint& a_pt)
|
||
{
|
||
if (a_pt.z > 1E-4)
|
||
{
|
||
if (roi.left < 0)
|
||
{
|
||
roi.left = a_pt.x;
|
||
roi.right = a_pt.x;
|
||
roi.left = a_pt.y;
|
||
roi.right = a_pt.y;
|
||
}
|
||
else
|
||
{
|
||
if (roi.left > a_pt.x)
|
||
roi.left = a_pt.x;
|
||
if (roi.right < a_pt.x)
|
||
roi.right = a_pt.x;
|
||
if (roi.top > a_pt.y)
|
||
roi.top = a_pt.y;
|
||
if (roi.bottom < a_pt.y)
|
||
roi.bottom = a_pt.y;
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
void _updateRenge(SVzNLRange& range, int idx)
|
||
{
|
||
if (range.nMin < 0)
|
||
{
|
||
range.nMin = idx;
|
||
range.nMax = idx;
|
||
}
|
||
else
|
||
{
|
||
if (range.nMin > idx)
|
||
range.nMin = idx;
|
||
if (range.nMax < idx)
|
||
range.nMax = idx;
|
||
}
|
||
return;
|
||
}
|
||
|
||
bool _checkExist(int id, std::vector<int>& buff)
|
||
{
|
||
for (int i = 0; i < (int)buff.size(); i++)
|
||
{
|
||
if (id == buff[i])
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
void _searchNeighbours(
|
||
int selfId, int chkExtening,
|
||
int sLineIdx, int eLineIdx,
|
||
SVzNLRange ptIdxRange,
|
||
std::vector<std::vector<int>>& treeMask,
|
||
std::vector<int>& neighbours)
|
||
{
|
||
int lineNum = (int)treeMask.size();
|
||
int ptNum = treeMask[0].size();
|
||
for (int line = sLineIdx - chkExtening; line <= eLineIdx + chkExtening; line++)
|
||
{
|
||
if ((line >= 0) && (line < lineNum))
|
||
{
|
||
for (int ptIdx = ptIdxRange.nMin - chkExtening; ptIdx <= ptIdxRange.nMax + chkExtening; ptIdx++)
|
||
{
|
||
if ((ptIdx >= 0) && (ptIdx < ptNum))
|
||
{
|
||
if ((treeMask[line][ptIdx] >= 0) && (treeMask[line][ptIdx] != selfId))
|
||
{
|
||
bool isExist = _checkExist(treeMask[line][ptIdx], neighbours);
|
||
if (false == isExist)
|
||
neighbours.push_back(treeMask[line][ptIdx]);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
void WD_getHoleInfo(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const SSG_lineSegParam lineSegPara,
|
||
const SSG_outlierFilterParam filterParam,
|
||
const SSG_treeGrowParam growParam,
|
||
const double valieCommonNumRatio,
|
||
std::vector<SWD_segFeatureTree>& segTrees_v,
|
||
std::vector<SWD_segFeatureTree>& segTrees_h,
|
||
std::vector<SSG_intPair>& validObjects
|
||
)
|
||
{
|
||
int lineNum = (int)scanLines.size();
|
||
int linePtNum = (int)scanLines[0].size();
|
||
|
||
std::vector<std::vector<int>> pointMask;
|
||
pointMask.resize(lineNum);
|
||
|
||
//<2F><>ȡ<EFBFBD>հ<EFBFBD><D5B0>߶<EFBFBD><DFB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
std::vector<std::vector<SWD_segFeature>> holeGaps;
|
||
//<2F><>ȡ<EFBFBD>߶ζ˵<CEB6><CBB5><EFBFBD><EFBFBD><EFBFBD>
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 1047)
|
||
int kkk = 1;
|
||
|
||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||
pointMask[line].resize(lineData.size());
|
||
std::fill(pointMask[line].begin(), pointMask[line].end(), 0);//<2F><>ʼ<EFBFBD><CABC>Ϊ0
|
||
//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
|
||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||
|
||
std::vector<SSG_RUN> segs;
|
||
wd_getLineDataNullIntervals(lineData, lineSegPara, segs);
|
||
|
||
//<2F><>seg<65>˵<EFBFBD><CBB5><EFBFBD>Ϊ<EFBFBD><CEAA>Ե<EFBFBD>㡣<EFBFBD><E3A1A3><EFBFBD>˵<EFBFBD><CBB5><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><EFBFBD>ֱ<EFBFBD><EFBFBD><D7B5>ڲ<EFBFBD><DAB2><EFBFBD>XYƽ<59><C6BD><EFBFBD>Ͼ<EFBFBD>Ϊ<EFBFBD><CEAA>Ե<EFBFBD>㡣
|
||
std::vector<SWD_segFeature> line_gaps;
|
||
for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
|
||
{
|
||
int ptIdx_1 = segs[i].start;
|
||
int ptIdx_2 = segs[i].start + segs[i].len - 1;
|
||
SWD_segFeature a_gap;
|
||
a_gap.lineIdx = line;
|
||
a_gap.startPtIdx = ptIdx_1;
|
||
a_gap.endPtIdx = ptIdx_2;
|
||
a_gap.startPt = lineData[ptIdx_1].pt3D;
|
||
a_gap.endPt = lineData[ptIdx_2].pt3D;
|
||
a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y);
|
||
line_gaps.push_back(a_gap);
|
||
}
|
||
holeGaps.push_back(line_gaps);
|
||
}
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
wd_getSegFeatureGrowingTrees_2(holeGaps, segTrees_v, growParam);
|
||
|
||
//<2F><><EFBFBD><EFBFBD>ˮƽɨ<C6BD><C9A8>
|
||
std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
|
||
hLines_raw.resize(linePtNum);
|
||
for (int i = 0; i < linePtNum; i++)
|
||
hLines_raw[i].resize(lineNum);
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
scanLines[line][j].nPointIdx = 0; //<2F><>ԭʼ<D4AD><CABC><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ʹ<EFBFBD>ã<EFBFBD>
|
||
hLines_raw[j][line] = scanLines[line][j];
|
||
hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
||
hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
||
}
|
||
}
|
||
//ˮƽarc<72><63><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
|
||
std::vector<std::vector<SWD_segFeature>> holeGaps_h;
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
if (line == 974)
|
||
int kkk = 1;
|
||
std::vector<SVzNL3DPosition>& lineData = hLines_raw[line];
|
||
//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
|
||
int ptNum = (int)lineData.size();
|
||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam);
|
||
|
||
std::vector<SSG_RUN> segs;
|
||
wd_getLineDataNullIntervals(lineData, lineSegPara, segs);
|
||
|
||
//<2F><>seg<65>˵<EFBFBD><CBB5><EFBFBD>Ϊ<EFBFBD><CEAA>Ե<EFBFBD>㡣<EFBFBD><E3A1A3><EFBFBD>˵<EFBFBD><CBB5><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><EFBFBD>ֱ<EFBFBD><EFBFBD><D7B5>ڲ<EFBFBD><DAB2><EFBFBD>XYƽ<59><C6BD><EFBFBD>Ͼ<EFBFBD>Ϊ<EFBFBD><CEAA>Ե<EFBFBD>㡣
|
||
std::vector<SWD_segFeature> line_gaps;
|
||
for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
|
||
{
|
||
int ptIdx_1 = segs[i].start;
|
||
int ptIdx_2 = segs[i].start + segs[i].len - 1;
|
||
SWD_segFeature a_gap;
|
||
a_gap.lineIdx = line;
|
||
a_gap.startPtIdx = ptIdx_1;
|
||
a_gap.endPtIdx = ptIdx_2;
|
||
a_gap.startPt = lineData[ptIdx_1].pt3D;
|
||
a_gap.endPt = lineData[ptIdx_2].pt3D;
|
||
a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y);
|
||
line_gaps.push_back(a_gap);
|
||
}
|
||
holeGaps_h.push_back(line_gaps);
|
||
}
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
wd_getSegFeatureGrowingTrees_2(holeGaps_h, segTrees_h, growParam);
|
||
|
||
//<2F><><EFBFBD><EFBFBD>Tree<65><65><EFBFBD>ڿ<DABF><D7B6><EFBFBD>Mask
|
||
std::vector<std::vector<int>> treeMask_v;
|
||
treeMask_v.resize(lineNum);
|
||
std::vector<std::vector<int>> treeMask_h;
|
||
treeMask_h.resize(lineNum);
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
treeMask_v[i].resize(linePtNum);
|
||
std::fill(treeMask_v[i].begin(), treeMask_v[i].end(), -1);
|
||
treeMask_h[i].resize(linePtNum);
|
||
std::fill(treeMask_h[i].begin(), treeMask_h[i].end(), -1);
|
||
}
|
||
//<2F><>ע
|
||
std::vector<SSG_treeInfo> treeInfo_v;
|
||
treeInfo_v.resize(segTrees_v.size());
|
||
for (int i = 0; i < (int)segTrees_v.size(); i++)
|
||
{
|
||
SWD_segFeatureTree& a_tree = segTrees_v[i];
|
||
treeInfo_v[i].treeIdx = i;
|
||
treeInfo_v[i].sLineIdx = a_tree.sLineIdx;
|
||
treeInfo_v[i].eLineIdx = a_tree.eLineIdx;
|
||
treeInfo_v[i].vTreeFlag = 0;
|
||
treeInfo_v[i].treeType = 0;
|
||
treeInfo_v[i].roi = { -1, -1, -1, -1 };
|
||
int nullPtSize = 0;
|
||
SSG_ROIRectD roi = { -1, -1, -1, -1 };
|
||
SVzNLRange ptIdxRange = { -1, -1 };
|
||
if (a_tree.treeNodes.size() > 0)
|
||
{
|
||
for (int m = 0; m < (int)a_tree.treeNodes.size(); m++)
|
||
{
|
||
SWD_segFeature& a_seg = a_tree.treeNodes[m];
|
||
for (int n = a_seg.startPtIdx; n <= a_seg.endPtIdx; n++)
|
||
treeMask_v[a_seg.lineIdx][n] = i;
|
||
|
||
nullPtSize += a_seg.endPtIdx - a_seg.startPtIdx + 1;
|
||
_updateROI(roi, scanLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D);
|
||
_updateROI(roi, scanLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D);
|
||
_updateRenge(ptIdxRange, a_seg.startPtIdx);
|
||
_updateRenge(ptIdxRange, a_seg.endPtIdx);
|
||
//scanLinesInput[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = 0x01;
|
||
//scanLinesInput[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = 0x01;
|
||
}
|
||
treeInfo_v[i].treeType = nullPtSize;
|
||
treeInfo_v[i].roi = roi;
|
||
treeInfo_v[i].ptIdxRange = ptIdxRange;
|
||
}
|
||
}
|
||
std::vector<SSG_treeInfo> treeInfo_h;
|
||
treeInfo_h.resize(segTrees_h.size());
|
||
for (int i = 0; i < (int)segTrees_h.size(); i++)
|
||
{
|
||
SWD_segFeatureTree& a_tree = segTrees_h[i];
|
||
treeInfo_h[i].treeIdx = i;
|
||
treeInfo_h[i].sLineIdx = a_tree.sLineIdx;
|
||
treeInfo_h[i].eLineIdx = a_tree.eLineIdx;
|
||
treeInfo_h[i].vTreeFlag = 0;
|
||
treeInfo_h[i].treeType = 0;
|
||
treeInfo_h[i].roi = { -1, -1, -1, -1 };
|
||
int nullPtSize = 0;
|
||
SSG_ROIRectD roi = { -1, -1, -1, -1 };
|
||
SVzNLRange ptIdxRange = { -1, -1 };
|
||
if (a_tree.treeNodes.size() > 0)
|
||
{
|
||
for (int m = 0; m < (int)a_tree.treeNodes.size(); m++)
|
||
{
|
||
SWD_segFeature& a_seg = a_tree.treeNodes[m];
|
||
for (int n = a_seg.startPtIdx; n <= a_seg.endPtIdx; n++)
|
||
treeMask_h[n][a_seg.lineIdx] = i;
|
||
|
||
nullPtSize += a_seg.endPtIdx - a_seg.startPtIdx + 1;
|
||
_updateROI(roi, scanLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D);
|
||
_updateROI(roi, scanLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D);
|
||
_updateRenge(ptIdxRange, a_seg.startPtIdx);
|
||
_updateRenge(ptIdxRange, a_seg.endPtIdx);
|
||
//scanLinesInput[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx |= 0x02;
|
||
//scanLinesInput[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx |= 0x02;
|
||
}
|
||
treeInfo_h[i].treeType = nullPtSize;
|
||
treeInfo_h[i].roi = roi;
|
||
treeInfo_h[i].ptIdxRange = ptIdxRange;
|
||
}
|
||
}
|
||
//ˮƽ<CBAE>ʹ<EFBFBD>ֱĿ<D6B1><C4BF><EFBFBD>ϲ<EFBFBD>
|
||
int vTreeSize = (int)segTrees_v.size();
|
||
int hTreeSize = (int)segTrees_h.size();
|
||
std::vector<std::vector<int>> treeHVInfo; //ͳ<>ƴ<EFBFBD>ֱ<EFBFBD><D6B1>ˮƽ<CBAE><C6BD>tree<65><65>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>Ϣ
|
||
treeHVInfo.resize(vTreeSize);
|
||
for (int i = 0; i < vTreeSize; i++)
|
||
{
|
||
treeHVInfo[i].resize(hTreeSize);
|
||
std::fill(treeHVInfo[i].begin(), treeHVInfo[i].end(), 0);
|
||
}
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
|
||
{
|
||
int idx_v = treeMask_v[line][ptIdx];
|
||
int idx_h = treeMask_h[line][ptIdx];
|
||
if ((idx_v >= 0) && (idx_h >= 0))
|
||
treeHVInfo[idx_v][idx_h]++;
|
||
}
|
||
}
|
||
|
||
//<2F><><EFBFBD>ɺ<EFBFBD>ѡĿ<D1A1><C4BF>
|
||
std::vector<SSG_intPair> objects;
|
||
for (int i = 0; i < vTreeSize; i++)
|
||
{
|
||
SWD_segFeatureTree& a_tree = segTrees_v[i];
|
||
if ((a_tree.sLineIdx <= 1047) && (a_tree.eLineIdx >= 1047))
|
||
int kkk = 1;
|
||
|
||
int totalSize_v = treeInfo_v[i].treeType;
|
||
int commonSize = 0;
|
||
int hTreeIdx = -1;
|
||
for (int j = 0; j < hTreeSize; j++)
|
||
{
|
||
if (commonSize < treeHVInfo[i][j])
|
||
{
|
||
hTreeIdx = j;
|
||
commonSize = treeHVInfo[i][j];
|
||
}
|
||
}
|
||
if (hTreeIdx >= 0)
|
||
{
|
||
int totalSize_h = treeInfo_h[hTreeIdx].treeType;
|
||
int sizeV_th = (int)((double)totalSize_v * valieCommonNumRatio);
|
||
int sizeH_th = (int)((double)totalSize_h * valieCommonNumRatio);
|
||
if ((commonSize > sizeH_th) && (commonSize > sizeV_th))
|
||
{
|
||
SSG_intPair a_obj;
|
||
a_obj.data_0 = i;
|
||
a_obj.data_1 = hTreeIdx;
|
||
a_obj.idx = commonSize;
|
||
objects.push_back(a_obj);
|
||
treeInfo_v[i].data = commonSize;
|
||
treeInfo_h[hTreeIdx].data = commonSize;
|
||
}
|
||
}
|
||
}
|
||
|
||
//<2F>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>ڡ<EFBFBD>ÿ<EFBFBD><C3BF>Ŀ<EFBFBD>걣<EFBFBD><EAB1A3>һ<EFBFBD><D2BB>
|
||
for (int i = 0; i < (int)objects.size(); i++)
|
||
{
|
||
int vTreeIdx = objects[i].data_0;
|
||
if (treeInfo_v[vTreeIdx].vTreeFlag < 0)
|
||
continue;
|
||
|
||
std::vector<int> neighbours;
|
||
_searchNeighbours(vTreeIdx, 3,
|
||
treeInfo_v[vTreeIdx].sLineIdx, treeInfo_v[vTreeIdx].eLineIdx,
|
||
treeInfo_v[vTreeIdx].ptIdxRange, treeMask_v, neighbours);
|
||
|
||
int bestIdx = vTreeIdx;
|
||
int maxValue = treeInfo_v[vTreeIdx].data;
|
||
if (neighbours.size() > 0)
|
||
{
|
||
for (int j = 0; j < (int)neighbours.size(); j++)
|
||
{
|
||
int idx = neighbours[j];
|
||
if (maxValue < treeInfo_v[idx].data)
|
||
{
|
||
maxValue = treeInfo_v[idx].data;
|
||
bestIdx = idx;
|
||
}
|
||
}
|
||
if (bestIdx != vTreeIdx)
|
||
treeInfo_v[vTreeIdx].vTreeFlag = -1;
|
||
for (int j = 0; j < (int)neighbours.size(); j++)
|
||
{
|
||
int idx = neighbours[j];
|
||
if (bestIdx != idx)
|
||
treeInfo_v[idx].vTreeFlag = -1;
|
||
}
|
||
}
|
||
}
|
||
|
||
for (int i = 0; i < (int)objects.size(); i++)
|
||
{
|
||
int vTreeIdx = objects[i].data_0;
|
||
if (treeInfo_v[vTreeIdx].vTreeFlag < 0)
|
||
continue;
|
||
|
||
validObjects.push_back(objects[i]);
|
||
}
|
||
}
|