合并 粒径算法 ver_1.0.0 修改导出符号并编译Arm版本

BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp

@@ -81,6 +81,27 @@ void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector<s
 	return;
 }
 
+void wdSavePlyTxt(const char* fileName, std::vector<std::vector< SVzNL3DPosition>> scanLines)
+{
+	std::ofstream sw(fileName);
+	int lineNum = scanLines.size();
+	for (int line = 0; line < lineNum; line++)
+	{
+		int nPositionCnt = scanLines[line].size();
+		for (int i = 0; i < nPositionCnt; i++)
+		{
+			SVzNL3DPoint* pt3D = &scanLines[line][i].pt3D;
+			if (pt3D->z < 1e-4)
+				continue;
+			double x = (double)pt3D->x;
+			double y = (double)pt3D->y;
+			double z = (double)pt3D->z;
+			sw << x << "," << y << "," << z << std::endl;
+		}
+	}
+	sw.close();
+}
+
 void _convertToGridData_XYZ_vector(std::vector<std::vector< SVzNL3DPosition>>& scanData, double _F, std::vector<std::vector< SVzNL3DPosition>>& scanData_grid)
 {
 	int min_y = 100000000;
@@ -619,7 +640,7 @@ int main()
 	};
 
 	SVzNLRange fileIdx[TEST_GROUP] = {
-		{1,3}, {6,8}, {6,8}, {6,8}, {6,8}, {9,12}
+		{1,3}, {6,8}, {6,8}, {6,8}, {6,8}, {9,11}
 	};
 
 #if CONVERT_TO_GRID
@@ -755,6 +776,10 @@ int main()
 			std::vector<std::vector< SVzNL3DPosition>> scanLines;
 			vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
 
+			//转成plyTxt格式
+			//sprintf_s(_scan_file, "%s%d_ply_Hi229229.txt", dataPath[grp], fidx);
+			//wdSavePlyTxt(_scan_file, scanLines);
+
 			long t1 = (long)GetTickCount64();//统计时间
 
 			for (int i = 0, i_max = (int)scanLines.size(); i < i_max; i++)

CMakeLists.txt

@@ -42,6 +42,8 @@ ADD_LIBRARY(beltTearingDetection SHARED sourceCode/beltTearingDetection.cpp)
 
 ADD_LIBRARY(workpieceCornerExtraction SHARED sourceCode/BQ_workpieceCornerExtraction.cpp)
 
+ADD_LIBRARY(particleSizeMeasurement SHARED sourceCode/WD_particleSizeMeasure.cpp)
+
 #add executable file
 # ADD_EXECUTABLE(bagPositioning_test bagPositioning_test/bagPositioning_test.cpp)
 

particleSizeMeasurement/particleSizeMeasurement.vcxproj

@@ -0,0 +1,172 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <ItemGroup Label="ProjectConfigurations">
+    <ProjectConfiguration Include="Debug|Win32">
+      <Configuration>Debug</Configuration>
+      <Platform>Win32</Platform>
+    </ProjectConfiguration>
+    <ProjectConfiguration Include="Release|Win32">
+      <Configuration>Release</Configuration>
+      <Platform>Win32</Platform>
+    </ProjectConfiguration>
+    <ProjectConfiguration Include="Debug|x64">
+      <Configuration>Debug</Configuration>
+      <Platform>x64</Platform>
+    </ProjectConfiguration>
+    <ProjectConfiguration Include="Release|x64">
+      <Configuration>Release</Configuration>
+      <Platform>x64</Platform>
+    </ProjectConfiguration>
+  </ItemGroup>
+  <ItemGroup>
+    <ClCompile Include="..\sourceCode\WD_particleSizeMeasure.cpp" />
+  </ItemGroup>
+  <ItemGroup>
+    <ClInclude Include="..\sourceCode\WD_particleSizeMeasure_Export.h" />
+  </ItemGroup>
+  <PropertyGroup Label="Globals">
+    <VCProjectVersion>16.0</VCProjectVersion>
+    <Keyword>Win32Proj</Keyword>
+    <ProjectGuid>{8fd0f574-61c6-4094-8521-33aeddb44797}</ProjectGuid>
+    <RootNamespace>particleSizeMeasurement</RootNamespace>
+    <WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
+  </PropertyGroup>
+  <Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
+    <ConfigurationType>DynamicLibrary</ConfigurationType>
+    <UseDebugLibraries>true</UseDebugLibraries>
+    <PlatformToolset>v142</PlatformToolset>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
+    <ConfigurationType>DynamicLibrary</ConfigurationType>
+    <UseDebugLibraries>false</UseDebugLibraries>
+    <PlatformToolset>v142</PlatformToolset>
+    <WholeProgramOptimization>true</WholeProgramOptimization>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
+    <ConfigurationType>DynamicLibrary</ConfigurationType>
+    <UseDebugLibraries>true</UseDebugLibraries>
+    <PlatformToolset>v142</PlatformToolset>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
+    <ConfigurationType>DynamicLibrary</ConfigurationType>
+    <UseDebugLibraries>false</UseDebugLibraries>
+    <PlatformToolset>v142</PlatformToolset>
+    <WholeProgramOptimization>true</WholeProgramOptimization>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
+  <ImportGroup Label="ExtensionSettings">
+  </ImportGroup>
+  <ImportGroup Label="Shared">
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <PropertyGroup Label="UserMacros" />
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
+    <LinkIncremental>true</LinkIncremental>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
+    <LinkIncremental>false</LinkIncremental>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
+    <LinkIncremental>true</LinkIncremental>
+    <OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
+    <IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
+    <LinkIncremental>false</LinkIncremental>
+    <OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
+    <IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
+  </PropertyGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <SDLCheck>true</SDLCheck>
+      <PreprocessorDefinitions>WIN32;_DEBUG;PARTICLESIZEMEASUREMENT_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+      <PrecompiledHeader>Use</PrecompiledHeader>
+      <PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
+    </ClCompile>
+    <Link>
+      <SubSystem>Windows</SubSystem>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+      <EnableUAC>false</EnableUAC>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <FunctionLevelLinking>true</FunctionLevelLinking>
+      <IntrinsicFunctions>true</IntrinsicFunctions>
+      <SDLCheck>true</SDLCheck>
+      <PreprocessorDefinitions>WIN32;NDEBUG;PARTICLESIZEMEASUREMENT_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+      <PrecompiledHeader>Use</PrecompiledHeader>
+      <PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
+    </ClCompile>
+    <Link>
+      <SubSystem>Windows</SubSystem>
+      <EnableCOMDATFolding>true</EnableCOMDATFolding>
+      <OptimizeReferences>true</OptimizeReferences>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+      <EnableUAC>false</EnableUAC>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <SDLCheck>true</SDLCheck>
+      <PreprocessorDefinitions>_DEBUG;PARTICLESIZEMEASUREMENT_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+      <PrecompiledHeader>NotUsing</PrecompiledHeader>
+      <PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
+      <AdditionalOptions>/D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions)</AdditionalOptions>
+    </ClCompile>
+    <Link>
+      <SubSystem>Windows</SubSystem>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+      <EnableUAC>false</EnableUAC>
+      <AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
+      <AdditionalDependencies>opencv_world320d.lib;baseAlgorithm.lib;%(AdditionalDependencies)</AdditionalDependencies>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <FunctionLevelLinking>true</FunctionLevelLinking>
+      <IntrinsicFunctions>true</IntrinsicFunctions>
+      <SDLCheck>true</SDLCheck>
+      <PreprocessorDefinitions>NDEBUG;PARTICLESIZEMEASUREMENT_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+      <PrecompiledHeader>NotUsing</PrecompiledHeader>
+      <PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
+      <AdditionalOptions>/D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions)</AdditionalOptions>
+    </ClCompile>
+    <Link>
+      <SubSystem>Windows</SubSystem>
+      <EnableCOMDATFolding>true</EnableCOMDATFolding>
+      <OptimizeReferences>true</OptimizeReferences>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+      <EnableUAC>false</EnableUAC>
+      <AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
+      <AdditionalDependencies>opencv_world320.lib;baseAlgorithm.lib;%(AdditionalDependencies)</AdditionalDependencies>
+    </Link>
+  </ItemDefinitionGroup>
+  <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
+  <ImportGroup Label="ExtensionTargets">
+  </ImportGroup>
+</Project>

particleSizeMeasurement_test/particleSizeMeasurement_test.cpp

@@ -0,0 +1,632 @@
+// BQ_workpieceCornerExtract_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
+//
+
+#include <iostream>
+#include <fstream>
+#include <vector>
+#include <stdio.h>
+#include <VZNL_Types.h>
+#include "direct.h"
+#include <string>
+#include "WD_particleSizeMeasure_Export.h"
+#include <opencv2/opencv.hpp>
+#include <Windows.h>
+#include <limits>
+
+typedef struct
+{
+	int r;
+	int g;
+	int b;
+}SG_color;
+
+typedef struct
+{
+	int nPointIdx;
+	double x;
+	double y;
+	double z;
+	float r;
+	float g;
+	float b;
+} SPointXYZRGB;
+
+void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData)
+{
+	std::ifstream inputFile(fileName);
+	std::string linedata;
+
+	if (inputFile.is_open() == false)
+		return;
+
+	std::vector< SVzNL3DPosition> a_line;
+	int ptIdx = 0;
+	while (getline(inputFile, linedata))
+	{
+		if (0 == strncmp("Line_", linedata.c_str(), 5))
+		{
+			int ptSize = (int)a_line.size();
+			if (ptSize > 0)
+			{
+				scanData.push_back(a_line);
+			}
+			a_line.clear();
+			ptIdx = 0;
+		}
+		else if (0 == strncmp("{", linedata.c_str(), 1))
+		{
+			float X, Y, Z;
+			int imageY = 0;
+			float leftX, leftY;
+			float rightX, rightY;
+			sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
+			SVzNL3DPosition a_pt;
+			a_pt.pt3D.x = X;
+			a_pt.pt3D.y = Y;
+			a_pt.pt3D.z = Z;
+			a_pt.nPointIdx = ptIdx;
+			ptIdx++;
+			a_line.push_back(a_pt);
+		}
+	}
+	//last line
+	int ptSize = (int)a_line.size();
+	if (ptSize > 0)
+	{
+		scanData.push_back(a_line);
+		a_line.clear();
+	}
+
+	inputFile.close();
+	return;
+}
+
+void _convertToGridData_XYZ_vector(std::vector<std::vector< SVzNL3DPosition>>& scanData, double _F, std::vector<std::vector< SVzNL3DPosition>>& scanData_grid)
+{
+	int min_y = 100000000;
+	int max_y = -10000000;
+	int lineNum = scanData.size();
+	for (int line = 0; line < lineNum; line++)
+	{
+		std::vector< SVzNL3DPosition>& a_line = scanData[line];
+		int nPointCnt = a_line.size();
+		for (int i = 0; i < nPointCnt; i++)
+		{
+			SVzNL3DPosition* a_pt = &scanData[line][i];
+			if (a_pt->pt3D.z > 1e-4)
+			{
+				double v = _F * a_pt->pt3D.y / a_pt->pt3D.z + 2000;
+				a_pt->nPointIdx = (int)(v + 0.5);
+				max_y = max_y < (int)a_pt->nPointIdx ? (int)a_pt->nPointIdx : max_y;
+				min_y = min_y > (int)a_pt->nPointIdx ? (int)a_pt->nPointIdx : min_y;
+			}
+		}
+	}
+	if (min_y == 100000000)
+		return;
+
+	int pt_counter = max_y - min_y + 1;
+	for (int line = 0; line < lineNum; line++)
+	{
+		std::vector< SVzNL3DPosition> gridData;
+		gridData.resize(pt_counter);
+		for (int i = 0; i < pt_counter; i++)
+			gridData[i] = { 0,{ 0.0, 0.0, 0.0} };
+
+		std::vector< SVzNL3DPosition>& a_line = scanData[line];
+		int nPointCnt = a_line.size();
+		for (int i = 0; i < nPointCnt; i++)
+		{
+			SVzNL3DPosition a_pt = a_line[i];
+			if (a_pt.pt3D.z > 1e-4)
+			{
+				int pt_id = a_pt.nPointIdx - min_y;
+				gridData[pt_id] = a_pt;
+			}
+		}
+		scanData_grid.push_back(gridData);
+	}
+	return;
+}
+
+void _outputScanDataFile_XYZ_vector(char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData)
+{
+	std::ofstream sw(fileName);
+	int lineNum = scanData.size();
+	sw << "LineNum:" << lineNum << std::endl;
+	sw << "DataType: 0" << std::endl;
+	sw << "ScanSpeed: 0" << std::endl;
+	sw << "PointAdjust: 1" << std::endl;
+	sw << "MaxTimeStamp: 0_0" << std::endl;
+
+	for (int line = 0; line < lineNum; line++)
+	{
+		int nPositionCnt = scanData[line].size();
+		sw << "Line_" << line << "_0_" << nPositionCnt << std::endl;
+		for (int i = 0; i < nPositionCnt; i++)
+		{
+			SVzNL3DPosition* pt3D = &scanData[line][i];
+			float x = (float)pt3D->pt3D.x;
+			float y = (float)pt3D->pt3D.y;
+			float z = (float)pt3D->pt3D.z;
+			char str[250];
+			sprintf_s(str, "{ %f, %f, %f } - { 0, 0 } - { 0, 0 }", x, y, z);
+
+			sw << str << std::endl;
+		}
+	}
+	sw.close();
+}
+
+void _getRoiData_XYZ_vector(
+	std::vector<std::vector< SVzNL3DPosition>>& scanData,
+	std::vector<std::vector< SVzNL3DPosition>>& roiData,
+	SVzNL3DRangeD roi)
+{
+	int lineNum = scanData.size();
+	for (int line = 0; line < lineNum; line++)
+	{
+		int nPositionCnt = scanData[line].size();
+		std::vector< SVzNL3DPosition> linePts;
+		for (int i = 0; i < nPositionCnt; i++)
+		{
+			SVzNL3DPosition pt3D = scanData[line][i];
+			if ((pt3D.pt3D.z >= roi.zRange.min) &&
+				(pt3D.pt3D.z <= roi.zRange.max) &&
+				(pt3D.pt3D.y >= roi.yRange.min) &&
+				(pt3D.pt3D.y <= roi.yRange.max))
+			{
+				linePts.push_back(pt3D);
+			}
+		}
+		roiData.push_back(linePts);
+	}
+}
+
+void _outputCalibPara(char* fileName, SSG_planeCalibPara calibPara)
+{
+	std::ofstream sw(fileName);
+	char dataStr[250];
+	//调平矩阵
+	sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[0], calibPara.planeCalib[1], calibPara.planeCalib[2]);
+	sw << dataStr << std::endl;
+	sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[3], calibPara.planeCalib[4], calibPara.planeCalib[5]);
+	sw << dataStr << std::endl;
+	sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[6], calibPara.planeCalib[7], calibPara.planeCalib[8]);
+	sw << dataStr << std::endl;
+	//地面高度
+	sprintf_s(dataStr, 250, "%g", calibPara.planeHeight);
+	sw << dataStr << std::endl;
+	//反向旋转矩阵
+	sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[0], calibPara.invRMatrix[1], calibPara.invRMatrix[2]);
+	sw << dataStr << std::endl;
+	sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[3], calibPara.invRMatrix[4], calibPara.invRMatrix[5]);
+	sw << dataStr << std::endl;
+	sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[6], calibPara.invRMatrix[7], calibPara.invRMatrix[8]);
+	sw << dataStr << std::endl;
+
+	sw.close();
+}
+
+void _outputParticleSizeInfo(char* fileName, std::vector<SWD_ParticlePosInfo>& particles)
+{
+	int particleSize = (int)particles.size();
+	if (particleSize == 0)
+		return;
+
+	std::ofstream sw(fileName);
+	char dataStr[250];
+	sprintf_s(dataStr, 250, "particleNum: %d", particleSize);
+	sw << dataStr << std::endl;
+
+	for (int i = 0; i < particleSize; i++)
+	{
+		sprintf_s(dataStr, 250, "  id_%d: (%g, %g, %g)", i, particles[i].size.length, particles[i].size.width, particles[i].size.height);
+		sw << dataStr << std::endl;
+	}
+	sw.close();
+}
+
+void _outputScanDataFile_vector(char* fileName, std::vector<std::vector<SVzNL3DPosition>>& scanLines, bool removeZeros, int* headNullLines)
+{
+	std::ofstream sw(fileName);
+	int lineNum = (int)scanLines.size();
+	if (lineNum == 0)
+		return;
+
+	sw << "LineNum:" << lineNum << std::endl;
+	sw << "DataType: 0" << std::endl;
+	sw << "ScanSpeed: 0" << std::endl;
+	sw << "PointAdjust: 1" << std::endl;
+	sw << "MaxTimeStamp: 0_0" << std::endl;
+
+	int lineIdx = 0;
+	int null_lines = 0;
+	bool counterNull = true;
+	for (int line = 0; line < lineNum; line++)
+	{
+		int linePtNum = (int)scanLines[line].size();
+		if (linePtNum == 0)
+			continue;
+
+		if (true == removeZeros)
+		{
+			int vldPtNum = 0;
+			for (int i = 0; i < linePtNum; i++)
+			{
+				if (scanLines[line][i].pt3D.z > 1e-4)
+					vldPtNum++;
+			}
+			linePtNum = vldPtNum;
+		}
+		sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
+		lineIdx++;
+		bool isNull = true;
+		for (int i = 0; i < linePtNum; i++)
+		{
+			SVzNL3DPoint* pt3D = &scanLines[line][i].pt3D;
+			if ((pt3D->z > 1e-4) && (isNull == true))
+				isNull = false;
+			if ((true == removeZeros) && (pt3D->z < 1e-4))
+				continue;
+			float x = (float)pt3D->x;
+			float y = (float)pt3D->y;
+			float z = (float)pt3D->z;
+			sw << "{ " << x << "," << y << "," << z << " }-";
+			sw << "{0,0}-{0,0}" << std::endl;
+		}
+		if (true == counterNull)
+		{
+			if (true == isNull)
+				null_lines++;
+			else
+				counterNull = false;
+		}
+	}
+	*headNullLines = null_lines;
+	sw.close();
+}
+
+SSG_planeCalibPara _readCalibPara(char* fileName)
+{
+	//设置初始结果
+	double initCalib[9] = {
+		1.0, 0.0, 0.0,
+		0.0, 1.0, 0.0,
+		0.0, 0.0, 1.0 };
+	SSG_planeCalibPara planePara;
+	for (int i = 0; i < 9; i++)
+		planePara.planeCalib[i] = initCalib[i];
+	planePara.planeHeight = -1.0;
+	for (int i = 0; i < 9; i++)
+		planePara.invRMatrix[i] = initCalib[i];
+
+	std::ifstream inputFile(fileName);
+	std::string linedata;
+
+	if (inputFile.is_open() == false)
+		return planePara;
+
+	//调平矩阵
+	std::getline(inputFile, linedata);
+	sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[0], &planePara.planeCalib[1], &planePara.planeCalib[2]);
+	std::getline(inputFile, linedata);
+	sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[3], &planePara.planeCalib[4], &planePara.planeCalib[5]);
+	std::getline(inputFile, linedata);
+	sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[6], &planePara.planeCalib[7], &planePara.planeCalib[8]);
+	//地面高度
+	std::getline(inputFile, linedata);
+	sscanf_s(linedata.c_str(), "%lf", &planePara.planeHeight);
+	//反向旋转矩阵
+	std::getline(inputFile, linedata);
+	sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[0], &planePara.invRMatrix[1], &planePara.invRMatrix[2]);
+	std::getline(inputFile, linedata);
+	sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[3], &planePara.invRMatrix[4], &planePara.invRMatrix[5]);
+	std::getline(inputFile, linedata);
+	sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[6], &planePara.invRMatrix[7], &planePara.invRMatrix[8]);
+
+	inputFile.close();
+	return planePara;
+}
+
+void _outputScanDataResult_RGBD(
+	char* fileName,
+	std::vector<std::vector<SVzNL3DPosition>>& scanLines,
+	std::vector<SWD_ParticlePosInfo>& particles)
+{
+	int lineNum = (int)scanLines.size();
+	std::ofstream sw(fileName);
+	int realLines = lineNum;
+	if (particles.size() > 0)
+		realLines++;
+	sw << "LineNum:" << realLines << std::endl;
+	sw << "DataType: 0" << std::endl;
+	sw << "ScanSpeed: 0" << std::endl;
+	sw << "PointAdjust: 1" << std::endl;
+	sw << "MaxTimeStamp: 0_0" << std::endl;
+
+	int maxLineIndex = 0;
+	int max_stamp = 0;
+
+	SG_color rgb = { 0, 0, 0 };
+	int size = 1;
+	int lineIdx = 0;
+	for (int line = 0; line < lineNum; line++)
+	{
+		int linePtNum = (int)scanLines[line].size();
+		if (linePtNum == 0)
+			continue;
+
+		int vldNum = 0;
+		for (int i = 0; i < linePtNum; i++)
+		{
+			if (scanLines[line][i].pt3D.z > 1e-4)
+				vldNum++;
+		}
+
+		sw << "Line_" << lineIdx << "_0_" << vldNum << std::endl;
+		lineIdx++;
+		for (int i = 0; i < linePtNum; i++)
+		{
+			if (scanLines[line][i].pt3D.z <= 1e-4)
+				continue;
+
+			SVzNL3DPosition* pt3D = &scanLines[line][i];
+			int mkID = pt3D->nPointIdx;
+			if (mkID < 2)
+			{
+				rgb = { 200, 200, 200 };
+				size = 1;
+			}
+			else
+			{
+				rgb.r = (mkID * 97) % 256;
+				rgb.g = (mkID * 73) % 256;
+				rgb.b = (mkID * 59) % 256;
+				size = 1;
+			}
+			float x = (float)pt3D->pt3D.x;
+			float y = (float)pt3D->pt3D.y;
+			float z = (float)pt3D->pt3D.z;
+			sw << "{" << x << "," << y << "," << z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
+		}
+	}
+
+	rgb = { 255,0,0 };
+	if (particles.size() > 0)
+	{
+		int objNum = (int)particles.size();
+		int lineIdx = 0;
+		for (int i = 0; i < objNum; i++)
+		{
+			sw << "Poly_" << lineIdx << "_2" << std::endl;
+			sw << "{" << particles[i].vertix[0].x << "," << particles[i].vertix[0].y << "," << particles[i].vertix[0].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			sw << "{" << particles[i].vertix[1].x << "," << particles[i].vertix[1].y << "," << particles[i].vertix[1].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			lineIdx++;
+
+			sw << "Poly_" << lineIdx << "_2" << std::endl;
+			sw << "{" << particles[i].vertix[1].x << "," << particles[i].vertix[1].y << "," << particles[i].vertix[1].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			sw << "{" << particles[i].vertix[2].x << "," << particles[i].vertix[2].y << "," << particles[i].vertix[2].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			lineIdx++;
+
+			sw << "Poly_" << lineIdx << "_2" << std::endl;
+			sw << "{" << particles[i].vertix[2].x << "," << particles[i].vertix[2].y << "," << particles[i].vertix[2].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			sw << "{" << particles[i].vertix[3].x << "," << particles[i].vertix[3].y << "," << particles[i].vertix[3].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			lineIdx++;
+
+			sw << "Poly_" << lineIdx << "_2" << std::endl;
+			sw << "{" << particles[i].vertix[3].x << "," << particles[i].vertix[3].y << "," << particles[i].vertix[3].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			sw << "{" << particles[i].vertix[0].x << "," << particles[i].vertix[0].y << "," << particles[i].vertix[0].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			lineIdx++;
+
+			sw << "Poly_" << lineIdx << "_2" << std::endl;
+			sw << "{" << particles[i].vertix[0].x << "," << particles[i].vertix[0].y << "," << particles[i].vertix[0].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			sw << "{" << particles[i].vertix[4].x << "," << particles[i].vertix[4].y << "," << particles[i].vertix[4].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			lineIdx++;
+
+			sw << "Poly_" << lineIdx << "_2" << std::endl;
+			sw << "{" << particles[i].vertix[1].x << "," << particles[i].vertix[1].y << "," << particles[i].vertix[1].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			sw << "{" << particles[i].vertix[5].x << "," << particles[i].vertix[5].y << "," << particles[i].vertix[5].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			lineIdx++;
+
+			sw << "Poly_" << lineIdx << "_2" << std::endl;
+			sw << "{" << particles[i].vertix[2].x << "," << particles[i].vertix[2].y << "," << particles[i].vertix[2].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			sw << "{" << particles[i].vertix[6].x << "," << particles[i].vertix[6].y << "," << particles[i].vertix[6].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			lineIdx++;
+
+			sw << "Poly_" << lineIdx << "_2" << std::endl;
+			sw << "{" << particles[i].vertix[3].x << "," << particles[i].vertix[3].y << "," << particles[i].vertix[3].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			sw << "{" << particles[i].vertix[7].x << "," << particles[i].vertix[7].y << "," << particles[i].vertix[7].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			lineIdx++;
+
+			sw << "Poly_" << lineIdx << "_2" << std::endl;
+			sw << "{" << particles[i].vertix[4].x << "," << particles[i].vertix[4].y << "," << particles[i].vertix[4].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			sw << "{" << particles[i].vertix[5].x << "," << particles[i].vertix[5].y << "," << particles[i].vertix[5].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			lineIdx++;
+
+			sw << "Poly_" << lineIdx << "_2" << std::endl;
+			sw << "{" << particles[i].vertix[5].x << "," << particles[i].vertix[5].y << "," << particles[i].vertix[5].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			sw << "{" << particles[i].vertix[6].x << "," << particles[i].vertix[6].y << "," << particles[i].vertix[6].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			lineIdx++;
+
+			sw << "Poly_" << lineIdx << "_2" << std::endl;
+			sw << "{" << particles[i].vertix[6].x << "," << particles[i].vertix[6].y << "," << particles[i].vertix[6].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			sw << "{" << particles[i].vertix[7].x << "," << particles[i].vertix[7].y << "," << particles[i].vertix[7].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			lineIdx++;
+
+			sw << "Poly_" << lineIdx << "_2" << std::endl;
+			sw << "{" << particles[i].vertix[7].x << "," << particles[i].vertix[7].y << "," << particles[i].vertix[7].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			sw << "{" << particles[i].vertix[4].x << "," << particles[i].vertix[4].y << "," << particles[i].vertix[4].z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+			lineIdx++;
+		}
+		sw << "Poly_" << lineIdx << "_2" << std::endl;
+		sw << "{" << particles[0].vertix[0].x << "," << particles[0].vertix[0].y << "," << particles[0].vertix[0].z << "}-";
+		sw << "{0,0}-{0,0}-";
+		sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+		sw << "{" << particles[0].vertix[1].x << "," << particles[0].vertix[1].y << "," << particles[0].vertix[1].z << "}-";
+		sw << "{0,0}-{0,0}-";
+		sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
+		lineIdx++;
+	}
+
+	sw.close();
+}
+
+#define CONVERT_TO_GRID				0
+#define TEST_COMPUTE_CALIB_PARA		0
+#define TEST_COMPUTE_PARTICE_SIZE	1
+#define TEST_GROUP 1
+int main()
+{
+	const char* dataPath[TEST_GROUP] = {
+		"F:\\ShangGu\\粒径数据\\曝光\\3D数据\\"  //0
+	};
+
+	SVzNLRange fileIdx[TEST_GROUP] = {
+		{1,10}
+	};
+
+#if TEST_COMPUTE_PARTICE_SIZE
+	for (int grp = 0; grp <= 0; grp++)
+	{
+		SSG_planeCalibPara poseCalibPara;
+		//初始化成单位阵
+		poseCalibPara.planeCalib[0] = 1.0;
+		poseCalibPara.planeCalib[1] = 0.0;
+		poseCalibPara.planeCalib[2] = 0.0;
+		poseCalibPara.planeCalib[3] = 0.0;
+		poseCalibPara.planeCalib[4] = 1.0;
+		poseCalibPara.planeCalib[5] = 0.0;
+		poseCalibPara.planeCalib[6] = 0.0;
+		poseCalibPara.planeCalib[7] = 0.0;
+		poseCalibPara.planeCalib[8] = 1.0;
+		poseCalibPara.planeHeight = 2600.0;
+		for (int i = 0; i < 9; i++)
+			poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i];
+		char calibFile[250];
+#if 0
+		if (grp == 0)
+		{
+			sprintf_s(calibFile, "F:\\ShangGu\\粒径数据\\曝光\\3D数据\\ground_calib_para.txt");
+			poseCalibPara = _readCalibPara(calibFile);
+		}
+#endif
+		for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
+		{
+			//fidx =1;
+			char _scan_file[256];
+			sprintf_s(_scan_file, "%sgridScanData_%d.txt", dataPath[grp], fidx);
+			std::vector<std::vector< SVzNL3DPosition>> scanLines;
+			vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
+
+			long t1 = (long)GetTickCount64();//统计时间
+			for (int i = 0, i_max = (int)scanLines.size(); i < i_max; i++)
+			{
+				if (i == 14)
+					int kkk = 1;
+				//行处理
+				//调平，去除地面
+				wd_lineDataR(scanLines[i], poseCalibPara.planeCalib, -1);
+			}
+
+			SWD_paricleSizeParam paricleSizeParam;
+			paricleSizeParam.minSize = { 100.0, 100.0, 100.0 };
+			paricleSizeParam.alarmSize = { 1000.0, 1000.0, 1000.0 };
+			SWD_PSM_algoParam algoParam;
+			algoParam.filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
+			algoParam.filterParam.outlierTh = 5;
+			algoParam.cornerParam.cornerTh = 60;  //45度角
+			algoParam.cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
+			algoParam.cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4;
+			algoParam.cornerParam.minEndingGap_z = 20;
+			algoParam.cornerParam.jumpCornerTh_1 = 10; //水平角度，小于此角度视为水平
+			algoParam.cornerParam.jumpCornerTh_2 = 60;
+			algoParam.growParam.maxLineSkipNum = 10;
+			algoParam.growParam.yDeviation_max = 10.0;
+			algoParam.growParam.maxSkipDistance = 10.0;
+			algoParam.growParam.zDeviation_max = 10.0;// algoParam.bagParam.bagH / 2; //袋子高度1/2
+			algoParam.growParam.minLTypeTreeLen = 100; //mm
+			algoParam.growParam.minVTypeTreeLen = 100; //mm
+
+			int errCode = 0;
+			std::vector<SWD_ParticlePosInfo> particles;
+			wd_particleSizeMeasure(
+				scanLines,
+				paricleSizeParam,
+				poseCalibPara,
+				algoParam,
+				particles,
+				&errCode);
+			long t2 = (long)GetTickCount64();
+			printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
+			//输出测试结果
+			sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
+			_outputScanDataResult_RGBD(_scan_file, scanLines, particles);
+			sprintf_s(calibFile, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx);
+			_outputParticleSizeInfo(calibFile, particles);
+		}
+	}
+#endif
+}
+
+// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
+// 调试程序: F5 或调试 >“开始调试”菜单
+
+// 入门使用技巧: 
+//   1. 使用解决方案资源管理器窗口添加/管理文件
+//   2. 使用团队资源管理器窗口连接到源代码管理
+//   3. 使用输出窗口查看生成输出和其他消息
+//   4. 使用错误列表窗口查看错误
+//   5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
+//   6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件
+

particleSizeMeasurement_test/particleSizeMeasurement_test.vcxproj

@@ -0,0 +1,159 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <ItemGroup Label="ProjectConfigurations">
+    <ProjectConfiguration Include="Debug|Win32">
+      <Configuration>Debug</Configuration>
+      <Platform>Win32</Platform>
+    </ProjectConfiguration>
+    <ProjectConfiguration Include="Release|Win32">
+      <Configuration>Release</Configuration>
+      <Platform>Win32</Platform>
+    </ProjectConfiguration>
+    <ProjectConfiguration Include="Debug|x64">
+      <Configuration>Debug</Configuration>
+      <Platform>x64</Platform>
+    </ProjectConfiguration>
+    <ProjectConfiguration Include="Release|x64">
+      <Configuration>Release</Configuration>
+      <Platform>x64</Platform>
+    </ProjectConfiguration>
+  </ItemGroup>
+  <PropertyGroup Label="Globals">
+    <VCProjectVersion>16.0</VCProjectVersion>
+    <Keyword>Win32Proj</Keyword>
+    <ProjectGuid>{0f4995a6-3978-4ef6-87ad-cc15ec9b007b}</ProjectGuid>
+    <RootNamespace>particleSizeMeasurementtest</RootNamespace>
+    <WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
+  </PropertyGroup>
+  <Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
+    <ConfigurationType>Application</ConfigurationType>
+    <UseDebugLibraries>true</UseDebugLibraries>
+    <PlatformToolset>v142</PlatformToolset>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
+    <ConfigurationType>Application</ConfigurationType>
+    <UseDebugLibraries>false</UseDebugLibraries>
+    <PlatformToolset>v142</PlatformToolset>
+    <WholeProgramOptimization>true</WholeProgramOptimization>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
+    <ConfigurationType>Application</ConfigurationType>
+    <UseDebugLibraries>true</UseDebugLibraries>
+    <PlatformToolset>v142</PlatformToolset>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
+    <ConfigurationType>Application</ConfigurationType>
+    <UseDebugLibraries>false</UseDebugLibraries>
+    <PlatformToolset>v142</PlatformToolset>
+    <WholeProgramOptimization>true</WholeProgramOptimization>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
+  <ImportGroup Label="ExtensionSettings">
+  </ImportGroup>
+  <ImportGroup Label="Shared">
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <PropertyGroup Label="UserMacros" />
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
+    <LinkIncremental>true</LinkIncremental>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
+    <LinkIncremental>false</LinkIncremental>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
+    <LinkIncremental>true</LinkIncremental>
+    <OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
+    <IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
+    <LinkIncremental>false</LinkIncremental>
+    <OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
+    <IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
+  </PropertyGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <SDLCheck>true</SDLCheck>
+      <PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+    </ClCompile>
+    <Link>
+      <SubSystem>Console</SubSystem>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <FunctionLevelLinking>true</FunctionLevelLinking>
+      <IntrinsicFunctions>true</IntrinsicFunctions>
+      <SDLCheck>true</SDLCheck>
+      <PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+    </ClCompile>
+    <Link>
+      <SubSystem>Console</SubSystem>
+      <EnableCOMDATFolding>true</EnableCOMDATFolding>
+      <OptimizeReferences>true</OptimizeReferences>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <SDLCheck>true</SDLCheck>
+      <PreprocessorDefinitions>_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+      <AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;</AdditionalIncludeDirectories>
+      <AdditionalOptions>/D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions)</AdditionalOptions>
+    </ClCompile>
+    <Link>
+      <SubSystem>Console</SubSystem>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+      <AdditionalDependencies>opencv_world320d.lib;particleSizeMeasurement.lib;%(AdditionalDependencies)</AdditionalDependencies>
+      <AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <FunctionLevelLinking>true</FunctionLevelLinking>
+      <IntrinsicFunctions>true</IntrinsicFunctions>
+      <SDLCheck>true</SDLCheck>
+      <PreprocessorDefinitions>NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+      <AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;</AdditionalIncludeDirectories>
+      <AdditionalOptions>/D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions)</AdditionalOptions>
+    </ClCompile>
+    <Link>
+      <SubSystem>Console</SubSystem>
+      <EnableCOMDATFolding>true</EnableCOMDATFolding>
+      <OptimizeReferences>true</OptimizeReferences>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+      <AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
+      <AdditionalDependencies>opencv_world320.lib;particleSizeMeasurement.lib;%(AdditionalDependencies)</AdditionalDependencies>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemGroup>
+    <ClCompile Include="particleSizeMeasurement_test.cpp" />
+  </ItemGroup>
+  <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
+  <ImportGroup Label="ExtensionTargets">
+  </ImportGroup>
+</Project>

sourceCode/BQ_workpieceCornerExtraction_Export.h

@@ -1,20 +1,6 @@
 #pragma once
 
-#if defined(_MSC_VER) || defined(WIN64) || defined(_WIN64) || defined(__WIN64__) || defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
+#include "SG_baseAlgo_Export.h"
-#  define Q_DECL_EXPORT __declspec(dllexport)
-#  define Q_DECL_IMPORT __declspec(dllimport)
-#else
-#  define Q_DECL_EXPORT     __attribute__((visibility("default")))
-#  define Q_DECL_IMPORT     __attribute__((visibility("default")))
-#endif
-
-#if defined(SG_API_LIBRARY)
-#  define SG_WORKPIECESHARED_EXPORT Q_DECL_EXPORT
-#else
-#  define SG_WORKPIECESHARED_EXPORT Q_DECL_IMPORT
-
-#endif
-
 #include "SG_baseDataType.h"
 #include <vector>
 
@@ -49,22 +35,22 @@ typedef struct
 }SSX_debugInfo;
 
 //读版本号
-SG_WORKPIECESHARED_EXPORT const char* wd_BQWorkpieceCornerVersion(void);
+SG_APISHARED_EXPORT const char* wd_BQWorkpieceCornerVersion(void);
 
 //计算一个平面调平参数。
 //数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
 //旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
-SG_WORKPIECESHARED_EXPORT SSG_planeCalibPara sx_BQ_getBaseCalibPara(
+SG_APISHARED_EXPORT SSG_planeCalibPara sx_BQ_getBaseCalibPara(
 	std::vector< std::vector<SVzNL3DPosition>>& scanLines);
 
 //相机姿态调平，并去除地面
-SG_WORKPIECESHARED_EXPORT void sx_BQ_lineDataR(
+SG_APISHARED_EXPORT void sx_BQ_lineDataR(
 	std::vector< SVzNL3DPosition>& a_line,
 	const double* camPoseR,
 	double groundH);
 
 //提取搭接焊缝
-SG_WORKPIECESHARED_EXPORT SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
+SG_APISHARED_EXPORT SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
 	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
 	const SSG_cornerParam cornerPara,
 	const SSG_outlierFilterParam filterParam,

sourceCode/SG_bagPositioning_Export.h

@@ -1,23 +1,10 @@
 #pragma once
 
-#if defined(_MSC_VER) || defined(WIN64) || defined(_WIN64) || defined(__WIN64__) || defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
-#  define Q_DECL_EXPORT __declspec(dllexport)
-#  define Q_DECL_IMPORT __declspec(dllimport)
-#else
-#  define Q_DECL_EXPORT     __attribute__((visibility("default")))
-#  define Q_DECL_IMPORT     __attribute__((visibility("default")))
-#endif
-
-#if defined(SG_API_LIBRARY)
-#  define SG_BAGSHARED_EXPORT Q_DECL_EXPORT
-#else
-#  define SG_BAGSHARED_EXPORT Q_DECL_IMPORT
-#endif
-
 #define OUTPUT_DEBUG	0
 #define BAG_ALGO_USE_CORNER_FEATURE	1
 #define RGN_HIST_SIZE	16 //目标颜色统计的数目
 
+#include "SG_baseAlgo_Export.h"
 #include "SG_baseDataType.h"
 #include <vector>
 
@@ -55,17 +42,17 @@ typedef struct
 }SSG_stackBaseParam;
 
 //数据调平
-SG_BAGSHARED_EXPORT void sg_lineDataR(
+SG_APISHARED_EXPORT void sg_lineDataR(
 	SVzNL3DLaserLine* a_line,
 	const double* camPoseR,
 	double groundH);
 
-SG_BAGSHARED_EXPORT void sg_lineDataR_RGBD(
+SG_APISHARED_EXPORT void sg_lineDataR_RGBD(
 	SVzNLXYZRGBDLaserLine* a_line,
 	const double* camPoseR,
 	double groundH);
 
-SG_BAGSHARED_EXPORT void sg_bagPositioning_lineProc(
+SG_APISHARED_EXPORT void sg_bagPositioning_lineProc(
 	SVzNL3DLaserLine* a_line, 
 	int lineIdx, 
 	int* errCode, 
@@ -74,7 +61,7 @@ SG_BAGSHARED_EXPORT void sg_bagPositioning_lineProc(
 	const SG_bagPositionParam algoParam);
 
 //获取编织袋中心抓取点姿态，从顶部抓取
-SG_BAGSHARED_EXPORT void sg_getBagPosition(
+SG_APISHARED_EXPORT void sg_getBagPosition(
 	SVzNL3DLaserLine* laser3DPoints, 
 	int lineNum, 
 	//std::vector<SSG_lineFeature>& all_vLineFeatures, 
@@ -83,7 +70,7 @@ SG_BAGSHARED_EXPORT void sg_getBagPosition(
 	const SSG_planeCalibPara poseCalibPara,
 	std::vector<SSG_peakRgnInfo>& objOps);
 
-SG_BAGSHARED_EXPORT void sg_getBagPositionAndOrientation(
+SG_APISHARED_EXPORT void sg_getBagPositionAndOrientation(
 	SVzNLXYZRGBDLaserLine* laser3DPoints,
 	int lineNum,
 	//std::vector<SSG_lineFeature>& all_vLineFeatures, 
@@ -100,13 +87,13 @@ SG_BAGSHARED_EXPORT void sg_getBagPositionAndOrientation(
 #endif
 	int* errCode);
 
-SG_BAGSHARED_EXPORT void sg_sideBagPosition(
+SG_APISHARED_EXPORT void sg_sideBagPosition(
 	SVzNL3DLaserLine* laser3DPoints,
 	int lineNum,
 	const SG_bagPositionParam algoParam,
 	std::vector<SSG_sideBagInfo>& objOps);
 
-SG_BAGSHARED_EXPORT void sg_getSideBagStackBasePosition(
+SG_APISHARED_EXPORT void sg_getSideBagStackBasePosition(
 	SVzNL3DLaserLine* laser3DPoints,
 	int lineNum,
 	const SSG_stackBaseParam stackBaseParam,
@@ -117,6 +104,6 @@ SG_BAGSHARED_EXPORT void sg_getSideBagStackBasePosition(
 //计算一个平面调平参数。
 //数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
 //旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
-SG_BAGSHARED_EXPORT SSG_planeCalibPara sg_getBagBaseCalibPara(
+SG_APISHARED_EXPORT SSG_planeCalibPara sg_getBagBaseCalibPara(
 	SVzNL3DLaserLine* laser3DPoints,
 	int lineNum);

sourceCode/SG_baseAlgo_Export.h

@@ -1,20 +1,19 @@
 #pragma once
-
-
 #if defined(_MSC_VER) || defined(WIN64) || defined(_WIN64) || defined(__WIN64__) || defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
-#  define Q_ALGO_DECL_EXPORT __declspec(dllexport)
+#  define Q_DECL_EXPORT __declspec(dllexport)
-#  define Q_ALGO_DECL_IMPORT __declspec(dllimport)
+#  define Q_DECL_IMPORT __declspec(dllimport)
 #else
-#  define Q_ALGO_DECL_EXPORT     __attribute__((visibility("default")))
+#  define Q_DECL_EXPORT     __attribute__((visibility("default")))
-#  define Q_ALGO_DECL_IMPORT     __attribute__((visibility("default")))
+#  define Q_DECL_IMPORT     __attribute__((visibility("default")))
 #endif
 
 #if defined(SG_API_LIBRARY)
-#  define SG_APISHARED_EXPORT Q_ALGO_DECL_EXPORT
+#  define SG_APISHARED_EXPORT Q_DECL_EXPORT
 #else
-#  define SG_APISHARED_EXPORT Q_ALGO_DECL_IMPORT
+#  define SG_APISHARED_EXPORT Q_DECL_IMPORT
 #endif
 
+
 #include "SG_baseDataType.h"
 #include <vector>
 #include <opencv2/opencv.hpp>
@@ -423,3 +422,10 @@ SG_APISHARED_EXPORT void pointClout2DProjection(
 
 //·ÖË®ÁëËã·¨
 SG_APISHARED_EXPORT void watershed(SWD_waterShedImage& img);
+// 根据输入的种子点进行分水岭算法
+SG_APISHARED_EXPORT void wd_seedWatershed(
+    SWD_waterShedImage& img, 
+    std::vector<SSG_2DValueI>& watershedSeeds, //种子点
+    int maxLevel, //最大水位
+    int startMakerID  //起始Marker的ID
+);

sourceCode/SG_regionGrow.cpp

@@ -1339,7 +1339,7 @@ void sg_getLocalPeaks_distTransform(cv::Mat& input, std::vector<SSG_2DValueI>& p
         for (int j = 0; j < winNum_cols; j++)
         {
             SVzNL2DPoint LTpos = { j * searchWin.searchW_pts / 2 , i * searchWin.seachW_lines / 2 };
-            SSG_2DValueI pkPos = { -1, -1, 0, 0 };
+            SSG_2DValueI pkPos = { -1, -1, 0, 0, 0};
             _findDistTransformPeak(input, LTpos, searchWin, &pkPos);
             if ((pkPos.x >= 0) && (pkPos.y >= 0))
             {

sourceCode/SX_lapWeldDetection_Export.h

@@ -1,20 +1,6 @@
 #pragma once
 
-#if defined(_MSC_VER) || defined(WIN64) || defined(_WIN64) || defined(__WIN64__) || defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__)
+#include "SG_baseAlgo_Export.h"
-#  define Q_DECL_EXPORT __declspec(dllexport)
-#  define Q_DECL_IMPORT __declspec(dllimport)
-#else
-#  define Q_DECL_EXPORT     __attribute__((visibility("default")))
-#  define Q_DECL_IMPORT     __attribute__((visibility("default")))
-#endif
-
-#if defined(SG_API_LIBRARY)
-#  define SG_WELDSHARED_EXPORT Q_DECL_EXPORT
-#else
-#  define SG_WELDSHARED_EXPORT Q_DECL_IMPORT
-#endif
-
-
 #include "SG_baseDataType.h"
 #include <vector>
 #include <opencv2/opencv.hpp>
@@ -36,17 +22,17 @@ typedef struct
 //计算一个平面调平参数。
 //数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
 //旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
-SG_WELDSHARED_EXPORT SSG_planeCalibPara sx_getBaseCalibPara(
+SG_APISHARED_EXPORT SSG_planeCalibPara sx_getBaseCalibPara(
 	std::vector< std::vector<SVzNL3DPosition>>& scanLines);
 
 //相机姿态调平，并去除地面
-SG_WELDSHARED_EXPORT void sx_lineDataR(
+SG_APISHARED_EXPORT void sx_lineDataR(
 	std::vector< SVzNL3DPosition>& a_line,
 	const double* camPoseR,
 	double groundH);
 
 //提取搭接焊缝
-SG_WELDSHARED_EXPORT void sx_getLapWeldPostion(
+SG_APISHARED_EXPORT void sx_getLapWeldPostion(
 	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
 	const SSG_cornerParam cornerPara,
 	SSG_treeGrowParam growParam,

sourceCode/WD_particleSizeMeasure.cpp

@@ -5,6 +5,12 @@
 #include <opencv2/opencv.hpp>
 #include <limits>
 
+std::string	m_strVersion = "1.0.0";
+const char* wd_particleSegVersion(void)
+{
+	return m_strVersion.c_str();
+}
+
 //计算一个平面调平参数。
 //数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
 //旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
@@ -401,7 +407,9 @@ void wd_particleSizeMeasure(
 	cv::imwrite("distTransformMask.png", maskImage);
 	cv::Mat dtImage;
 	cv::normalize(distTransform, dtImage, 0, 255, cv::NORM_MINMAX, CV_8U);
-	cv::imwrite("distTransform.png", dtImage);
+	cv::Mat dtImage_color;
+	cv::cvtColor(dtImage, dtImage_color, cv::COLOR_GRAY2BGR);
+	cv::imwrite("distTransform.png", dtImage_color);
 #endif
 
 	//寻找Peak。Peak确定后以Peak为种子点进行分水岭方法分割
@@ -423,18 +431,46 @@ void wd_particleSizeMeasure(
 	int filterSize = (int)filter_dt_peaks.size();
 	for (int i = 0; i < filterSize; i++)
 	{
+		SSG_2DValueI& obj_0 = filter_dt_peaks[i];
 		for (int j = i + 1; j < filterSize; j++)
 		{
+			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)  //合并
+			{
+				if (obj_0.valueD < obj_1.valueD)
+					obj_0.value = -1;
+				else
+					obj_1.value = -1;
+			}
+		}
+	}
 
+	//有效种子
+	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
 
 	//分水岭分割
-	
 	//扫描边界，建立相邻目标边界集合 
 	//通过目标相邻边界判断是否合并相邻目标
 
-
 	//获取最大最小值
 	double minVal, maxVal;
 	cv::Point minLoc, maxLoc;
@@ -446,13 +482,18 @@ void wd_particleSizeMeasure(
 	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));  // 初始化标记图为0
+	wsImg.markers.resize(wsImg.height, std::vector<int>(wsImg.width, 1));  // 初始化标记图为1,背景
 	int maxValue = (int)maxVal + 2;
+	int maxLevel = (int)(maxVal - minVal);
 	for (int i = 0; i < distTransform.rows; i++)
 	{
+		if (i == 758)
+			int kkk = 1;
 		float* rowPtr = distTransform.ptr<float>(i);
 		for (int j = 0; j < distTransform.cols; j++)
 		{
+			if (j == 171)
+				int kkk = 1;
 			float disValue = rowPtr[j];
 			if (disValue < 1e-4) //边界和背景
 				wsImg.gray[i][j] = maxValue;
@@ -463,9 +504,8 @@ void wd_particleSizeMeasure(
 			}
 		}
 	}
-	
+	int startMarkerID = 2;
-	watershed(wsImg);
+	wd_seedWatershed(wsImg, vld_dt_peaks, maxLevel, startMarkerID);
-
 #if OUTPUT_DEBUG  //debug	
 	cv::Mat waterShedResult(wsImg.height, wsImg.width, CV_8UC3);
 	for (int i = 0; i < wsImg.height; ++i) {
@@ -473,633 +513,104 @@ void wd_particleSizeMeasure(
 			if (wsImg.markers[i][j] == -1) {  // 分水岭边界（红色）
 				waterShedResult.at<cv::Vec3b>(i, j) = cv::Vec3b(0,0,255);
 			}
-			else {  // 区域（根据标记值生成不同颜色）
+			else if (wsImg.markers[i][j] < 2)
-				int color_r = (wsImg.markers[i][j] * 50) % 256;
+			{
-				int color_g = (color_r + 85) % 256;
+				waterShedResult.at<cv::Vec3b>(i, j) = cv::Vec3b(200, 200, 200);
-				int color_b = (color_r + 170) % 256;
+			}
+			else 
+			{  // 区域（根据标记值生成不同颜色）
+
+				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;
 				waterShedResult.at<cv::Vec3b>(i, j) = cv::Vec3b(color_b, color_g, color_r);
 			}
 		}
 	}
 	cv::imwrite("watershed.png", waterShedResult);
 #endif
-#if 0
 
-
+	//生成分割后的目标
-
+	//检查每个3D点在投影上的位置，生成目标的3D点
-
+	std::vector<std::vector< SVzNL3DPoint>> segObjs;
-
+	int maxMkID = startMarkerID + (int)vld_dt_peaks.size();
-
+	segObjs.resize(maxMkID);
-		SSG_localPkParam searchWin;
+	for (int line = 0; line < lineNum; line++)
-		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);
-		//获取Peaks
-		int invlidDistToEdge = 50; //距离边缘近的Peak是非法点。
-		double minPeakValue = algoParam.bagParam.bagW / 8;
-		std::vector<SSG_2DValueI> peaks;
-		for (int i = 0; i < dt_peaks.size(); i++)
 	{
-			//边界处的Peak为不合格Peak，去除
+		for (int i = 0; i < nPointCnt; i++)
-			int x_diff_0 = dt_peaks[i].x;  //距左边距离，单位为mm（量化尺度为1mm）
+		{
-			int x_diff_1 = distTransform.cols - dt_peaks[i].x;//距右边距离
+			SVzNL3DPosition* pt3D = &scanLines[line][i];
-			int y_diff_0 = dt_peaks[i].y;//距上边距离
+			pt3D->nPointIdx = 0;
-			int y_diff_1 = distTransform.rows - dt_peaks[i].y;//距下边距离
+			if (pt3D->pt3D.z < 1e-4)
-			if ((x_diff_0 < invlidDistToEdge) || (x_diff_1 < invlidDistToEdge) ||
-				(y_diff_0 < invlidDistToEdge) || (y_diff_1 < invlidDistToEdge) ||
-				(dt_peaks[i].valueD < minPeakValue))
 				continue;
 
-			//在distTranformIndexing中回找坐标
+			double x = pt3D->pt3D.x;
-			double pkValue = dt_peaks[i].valueD;
+			double y = pt3D->pt3D.y;
-			SSG_2DValueI a_peak = _backIndexingPeakPos(dt_peaks[i], distTranformIndexing);
+			int px = (int)(x - x_range.min) / scale + edgeSkip;
-			a_peak.valueD = pkValue; // laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y].pt3D.z;
+			int py = (int)(y - y_range.min) / scale + edgeSkip;
-			peaks.push_back(a_peak);
-		}
-		//按照高度排序
-		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
+			int marker = wsImg.markers[py][px];
-	//使用真实的z值代替距离变换值
+			if ((marker >= startMarkerID)&&( marker <= maxMkID))
-		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;
+				pt3D->nPointIdx = marker;
+				segObjs[marker].push_back(pt3D->pt3D);
+			}
+		}
 	}
-#endif
 
-		/// 以区域最高点作为种子进行区域生长
+	//生成目标
-		std::vector<SSG_peakRgnInfo> peakRgns;
+	for (int i = startMarkerID; i < maxMkID; i++)
-		int peakRgnId = 1;
-		for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++)
 	{
-			if (i == 3)
+		if (segObjs[i].size() < 10)
-				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;
 
-			//生长
+		//获取投影,并统计最小Z和最大Z
-			//进行水平和垂直方向扫描得到边界点 
+		double minZ = -1;
-			std::vector< SSG_lineConotours> topContour;
+		double maxZ = 0;
-			std::vector< SSG_lineConotours> bottomContour;
+		std::vector<cv::Point2f> points;
-			std::vector< SSG_lineConotours> leftContour;
+		int ptSize = (int)segObjs[i].size();
-			std::vector< SSG_lineConotours> rightContour;
+		for (int m = 0; m < ptSize; m++)
-			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;
-
-			//全匹配：对于褶皱较多的袋子，需要全匹配寻找到正确的边
-			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);
+			float x = (float)segObjs[i][m].x;
-				peakRgnId++;
+			float y = (float)segObjs[i][m].y;
-			}
+			points.push_back(cv::Point2f(x, y));
-		}
+			if (minZ < 0)
-#if 1
-		///迭代处理！！！！保证没有大于袋子的目标未处理
-		///对于剩下的目标，如果没有检出的使用推理方法进行。
-		///扫描时检测保证水平或垂直方向的宽度是否满足袋子尺寸，然后在另一个方向进行推理方法检测
-		while (1)
 			{
-			std::vector<SSG_peakRgnInfo> iter_objs;
+				minZ = segObjs[i][m].z;
-			//将没有处理的Peak点保留
+				maxZ = segObjs[i][m].z;
-			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;
-				//生长
-				//进行水平和垂直方向扫描得到边界点 
-				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;
-
-				//分段计算平均宽度和平均高度以及分段ROI
-				//全匹配（将水平所有分段的所有可能距离
-				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());
-			//为下一次迭代准备
-			peaks.clear();
-			peaks.insert(peaks.end(), residualPeaks.begin(), residualPeaks.end());
-		}
-
-		///将剩余的小目标检出，用于碰撞检测
-		///不能有未知的未处理的区域，以防止未知的碰撞
-		///记录所有的大于1/4L*1/4W的目标 
-		std::vector<SSG_2DValueI> smallObjPeaks;  //记录0.25L * 0.25W的目标，用于碰撞检查
-		//将最后没有处理的Peak点保留
-		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++)
-			{
-				//生长
-				//进行水平和垂直方向扫描得到边界点 
-				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);
-				//计算ROI
-				//保留长宽都大于门限的
-				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;
+				if (minZ > segObjs[i][m].z) minZ = segObjs[i][m].z;
-							objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right;
+				if (maxZ < segObjs[i][m].z) maxZ = segObjs[i][m].z;
-							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;
 			}
 		}
-				}
+		if (points.size() == 0)
-				for (int n = 0; n < resi_bottomContour.size(); n++)
+			continue;
+		//最小外接矩
+		// 最小外接矩形
+		cv::RotatedRect rect = minAreaRect(points);
+		cv::Point2f vertices[4];
+		rect.points(vertices);
+		double width = rect.size.width;  //投影的宽和高， 对应袋子的长和宽
+		double height = rect.size.height;
+		if (width < height)
 		{
-					std::vector<SSG_contourPtInfo>& a_line_contourPts = resi_bottomContour[n].contourPts;
+			double tmp = height;
-					for (int m = 0; m < a_line_contourPts.size(); m++)
+			height = width;
+			width = tmp;
+		}
+		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++)
 		{
-						if (objROI.right < objROI.left)
+			SVzNL3DPoint vPt_btm = { vertices[m].x, vertices[m].y, maxZ };
-						{
+			SVzNL3DPoint vPt_top = { vertices[m].x, vertices[m].y, minZ };
-							objROI.left = a_line_contourPts[m].edgePt.x;
+			a_obj.vertix[m] = vPt_btm;
-							objROI.right = a_line_contourPts[m].edgePt.x;
+			a_obj.vertix[m + 4] = vPt_top;
-							objROI.top = a_line_contourPts[m].edgePt.y;
-							objROI.bottom = a_line_contourPts[m].edgePt.y;
 		}
-						else
+		particles.push_back(a_obj);
-						{
-							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;
-						}
-					}
-				}
-				//检查ROI大小
-				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
-
-		//目标排序：分层 -> 对最高层分行 -> 对最高层第一行从左到右排序
-		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;
-			}
-			//取同高度层的目标
-			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)  //分层
-				{
-					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)  //进一步排序，分行
-			{
-				//取Y最小的目标
-				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) //第一行
-						objOps.push_back(peakRgns[i]);
-					else
-						row_0_outlier.push_back(i);  //将其它行的序号记录下来
-				}
-				//对第一行的目标按从左到右排序
-				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());
-		}
-		//碰撞检测
-		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));
-					//同层比较
-					double z_diff = smallPkPt->pt3D.z - objZ;
-					if (z_diff < algoParam.bagParam.bagH / 2)  //分层
-					{
-						if (dist < dia_angle / 2)
-							objOps.clear(); //本次检测无效
-					}
-				}
-			}
-		}
-		//将数据重新投射回原来的坐标系，以保持手眼标定结果正确
-		for (int i = 0; i < lineNum; i++)
-			sg_lineDataR(&laser3DPoints[i], poseCalibPara.invRMatrix, -1);
-		//将检测结果重新投射回原来的坐标系
-		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
-	//水平和垂直提取特征
-	
-	//进行距离变换
-	//分水岭分割
-	//按z高度检测目标
 }

sourceCode/WD_particleSizeMeasure_Export.h

@@ -1,16 +1,11 @@
 #pragma once
 
-#if defined(SG_API_LIBRARY)
+#include "SG_baseAlgo_Export.h"
-#  define SG_APISHARED_EXPORT __declspec(dllexport)
-#else
-#  define SG_APISHARED_EXPORT __declspec(dllimport)
-#endif
-
 #include "SG_baseDataType.h"
 #include <vector>
 #include <opencv2/opencv.hpp>
 
-#define OUTPUT_DEBUG	1
+#define OUTPUT_DEBUG	0
 
 typedef struct
 {
@@ -27,10 +22,14 @@ typedef struct
 
 typedef struct
 {
-	double EQRadius;
+	//double EQRadius;
-	SVzNL3DPoint center_pos;
+	SWD_sizeParam size;
+	SVzNL3DPoint vertix[8];
 }SWD_ParticlePosInfo;
 
+//读版本号
+SG_APISHARED_EXPORT const char* wd_particleSegVersion(void);
+
 //计算一个平面调平参数。
 //数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
 //旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数

sourceCode/WD_watershed.cpp

@@ -246,6 +246,9 @@ void saveResult(const string& filename, const Image& img) {
 const int dx[] = { -1, -1, -1, 0, 0, 1, 1, 1 };
 const int dy[] = { -1, 0, 1, -1, 1, -1, 0, 1 };
 
+const int dx4[] = { -1,  0, 0, 1};
+const int dy4[] = {  0, -1, 1, 0};
+
 // 计算局部极小值作为初始种子点
 void findMinima(SWD_waterShedImage& img, int& markerCount) {
     markerCount = 1; //背景对应的ID
@@ -311,7 +314,7 @@ void watershed(SWD_waterShedImage& img)
     vector<pair<int, pair<int, int>>> pixels;  // (灰度值, (x,y))
     for (int i = 0; i < img.height; ++i) {
         for (int j = 0; j < img.width; ++j) {
-            pixels.emplace_back(img.gray[i][j], make_pair(i, j));
+            pixels.emplace_back(img.gray[i][j], make_pair(j, i));
         }
     }
     sort(pixels.begin(), pixels.end());
@@ -348,19 +351,156 @@ void watershed(SWD_waterShedImage& img)
     }
 }
 
-#if 0
+//模拟注水，从注水口开始，将同水位注满
-int main() {
+// inlet: 注水口
-    Image img;
+// level: 水位
-    if (!readPPM("input.ppm", img)) {  // 输入PPM图像
+void waterInjection(SWD_waterShedImage& img, SVzNL2DPoint inlet, int level)
-        cerr << "无法读取图像！" << endl;
+{
-        return -1;
+    int py = inlet.y;
+    int px = inlet.x;
+    int marker = img.markers[py][px];
+    std::vector<SVzNL2DPoint> levelPts;
+    levelPts.push_back(inlet);
+    int readPtr = 0;
+    while (readPtr < levelPts.size())
+    {
+        SVzNL2DPoint seed = levelPts[readPtr];
+        readPtr++;
+
+        //4邻接
+        for (int d = 0; d < 4; ++d) 
+        {
+            int nx = seed.x + dx4[d];
+            int ny = seed.y + dy4[d];
+            if (ny >= 0 && ny < img.height && nx >= 0 && nx < img.width) 
+            {
+                if ((img.gray[ny][nx] == level) && (img.markers[ny][nx] == 0))
+                {
+                    img.markers[ny][nx] = marker;
+                    SVzNL2DPoint nxt_seed = { nx, ny };
+                    levelPts.push_back(nxt_seed);
+                }
+            }
+        }
+    }
+}
+
+bool _checkMarkerExist(std::vector<int>& MarkerLst, int marker)
+{
+    bool exist = false;
+    for (int i = 0, i_max = (int)MarkerLst.size(); i < i_max; i++)
+    {
+        if (MarkerLst[i] == marker)
+        {
+            exist = true;
+            break;
+        }
+    }
+    return exist;
+}
+
+// 根据输入的种子点进行分水岭算法
+// watershedSeeds:种子点
+// maxLevel:最大水位值
+void wd_seedWatershed(SWD_waterShedImage& img, std::vector<SSG_2DValueI>& watershedSeeds, int maxLevel, int startMakerID)
+{
+    int markerCount = startMakerID;
+    int seedSize = (int)watershedSeeds.size();
+    for (int i = 0; i < seedSize; i++)
+    {
+        int px = watershedSeeds[i].x;
+        int py = watershedSeeds[i].y;
+        watershedSeeds[i].value = markerCount;
+        img.markers[py][px] = markerCount;  // 新种子点
+        int greyValue = img.gray[py][px];  //水位
+        SVzNL2DPoint inlet = { px, py };  //注水口
+        waterInjection(img, inlet, greyValue);//注水，将同一水位连接的部分注满
+        markerCount++;
     }
 
-    watershed(img);  // 执行分水岭分割
+    // 按灰度值排序所有像素（模拟水位上升）
-    saveResult("watershed_result.ppm", img);  // 保存结果
+    std::vector<std::vector<SVzNL2DPoint>> levelPtList;
+    levelPtList.resize(maxLevel + 1);
+    for (int y = 0; y < img.height; ++y) 
+    {
+        for (int x = 0; x < img.width; ++x) 
+        {
+            int level = img.gray[y][x];
+            if (level <= maxLevel)
+            {
+                SVzNL2DPoint a_pt = { x, y };
+                levelPtList[level].push_back(a_pt);
+            }
+        }
+    }
 
-    cout << "分割完成，结果已保存为 watershed_result.ppm" << endl;
+    while (1)
-    return 0;
+    {
+        bool allDone = true;
+        // 遍历排序后的像素，执行注水过程
+        for (int i = 0; i <= maxLevel; i++)
+        {
+            if (levelPtList[i].size() == 0)
+                continue;
+
+            //对同一水位（level)执行迭代注水
+            bool doInjection = true;
+            while (doInjection)
+            {
+                int injectionNum = 0;
+                std::vector<SVzNL2DPoint> resiPts;
+                int ptSize = (int)levelPtList[i].size();
+                for (int pi = 0; pi < ptSize; pi++)
+                {
+                    int x = levelPtList[i][pi].x;
+                    int y = levelPtList[i][pi].y;
+                    if (img.markers[y][x] > 0) continue;  // 已标记的种子点
+
+                    // 收集邻域已标记的区域
+                    vector<int> neighborMarkers;
+                    for (int d = 0; d < 8; ++d)
+                    {
+                        int nx = x + dx[d];
+                        int ny = y + dy[d];
+                        if (ny >= 0 && ny < img.height && nx >= 0 && nx < img.width)
+                        {
+                            int m = img.markers[ny][nx];
+                            if (m > 0)
+                            {
+                                bool exist = _checkMarkerExist(neighborMarkers, m);
+                                if (false == exist)
+                                    neighborMarkers.push_back(m);
+                            }
+                        }
+                    }
+
+                    if (neighborMarkers.empty())
+                    {
+                        resiPts.push_back(levelPtList[i][pi]);// 无邻域标记，等待迭代
+                    }
+                    else if (neighborMarkers.size() == 1) {
+                        img.markers[y][x] = neighborMarkers[0];  // 属于同一区域
+                        injectionNum++;
+                    }
+                    else {
+                        // 多区域交汇，标记为分水岭
+                        img.markers[y][x] = -1;
+                        injectionNum++;
+                    }
+                }
+                levelPtList[i].clear();
+                levelPtList[i].insert(levelPtList[i].end(), resiPts.begin(), resiPts.end());
+
+                if ((injectionNum == 0) || (levelPtList[i].size() == 0))
+                {
+                    doInjection = false;
+                    if (levelPtList[i].size() > 0)
+                        allDone = false;
+                }
+            }
+        }
+        if (true == allDone)
+            break;
+    }
 }
 #endif
-#endif