From e0a8333bf29a6f8f3ca5a8322f7efb0d11d0e554 Mon Sep 17 00:00:00 2001
From: jerryzeng <laohong_0908@qq.com>
Date: Fri, 5 Dec 2025 23:04:40 +0800
Subject: [PATCH] -add noiseFilter

---
 sourceCode/WD_noiseFilter.cpp | 62 +++++++++++++++++++++++++++++++++++
 1 file changed, 62 insertions(+)
 create mode 100644 sourceCode/WD_noiseFilter.cpp

diff --git a/sourceCode/WD_noiseFilter.cpp b/sourceCode/WD_noiseFilter.cpp
new file mode 100644
index 0000000..269ea27
--- /dev/null
+++ b/sourceCode/WD_noiseFilter.cpp
@@ -0,0 +1,62 @@
+#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;
+}