63 lines
1.5 KiB
C++
63 lines
1.5 KiB
C++
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include <vector>
|
||
|
||
void wd_noiseFilter(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const SSG_outlierFilterParam filterParam,
|
||
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],
|
||
filterParam);
|
||
}
|
||
if (false == vldGrid)
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_INVLD;
|
||
return;
|
||
}
|
||
//水平方向过滤
|
||
int hLineNum = nPointCnt; //Grid格式，所有扫描线的点数是一样的
|
||
//生成水平扫描数据
|
||
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++)
|
||
{
|
||
//滤波，滤除异常点
|
||
std::vector<SVzNL3DPosition> filterData;
|
||
std::vector<int> lineNoise;
|
||
sg_lineDataRemoveOutlier(
|
||
(SVzNL3DPosition*)filterHLines[hLine].data(),
|
||
(int)filterHLines[hLine].size(),
|
||
filterParam,
|
||
filterData,
|
||
lineNoise);
|
||
for (int j = 0; j < lineNoise.size(); j++)
|
||
{
|
||
int lineIdx = lineNoise[j];
|
||
scanLines[lineIdx][hLine].pt3D.z = 0;
|
||
}
|
||
}
|
||
return;
|
||
}
|