添加了拆包算法中编织袋正反识别和朝向识别算法，以及相应的测试程序

QRCode 3D detection algorithm initial check in
QRCode（DM code) algorithm and test projects initial check in
2025-07-15 21:39:09 +08:00 · 2025-07-15 21:37:09 +08:00 · 2025-07-15 21:35:15 +08:00
17 changed files with 4787 additions and 104 deletions
--- a/QRcode3Ddetection/QRcode3Ddetection.vcxproj
+++ b/QRcode3Ddetection/QRcode3Ddetection.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_QRcode3Ddetection.cpp" />
+  </ItemGroup>
+  <ItemGroup>
+    <ClInclude Include="..\sourceCode\WD_QRcode3Ddetection_Export.h" />
+  </ItemGroup>
+  <PropertyGroup Label="Globals">
+    <VCProjectVersion>16.0</VCProjectVersion>
+    <Keyword>Win32Proj</Keyword>
+    <ProjectGuid>{a7c72ed9-b81d-4c6a-be38-d75199c824aa}</ProjectGuid>
+    <RootNamespace>QRcode3Ddetection</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;..\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\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
+  </PropertyGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <SDLCheck>true</SDLCheck>
+      <PreprocessorDefinitions>WIN32;_DEBUG;QRCODE3DDETECTION_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;QRCODE3DDETECTION_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>SG_API_LIBRARY;_DEBUG;QRCODE3DDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+      <PrecompiledHeader>NotUsing</PrecompiledHeader>
+      <PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
+      <AdditionalIncludeDirectories>..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
+    </ClCompile>
+    <Link>
+      <SubSystem>Windows</SubSystem>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+      <EnableUAC>false</EnableUAC>
+      <AdditionalLibraryDirectories>..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
+      <AdditionalDependencies>opencv_world480d.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>SG_API_LIBRARY;NDEBUG;QRCODE3DDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+      <PrecompiledHeader>NotUsing</PrecompiledHeader>
+      <PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
+      <AdditionalIncludeDirectories>..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
+    </ClCompile>
+    <Link>
+      <SubSystem>Windows</SubSystem>
+      <EnableCOMDATFolding>true</EnableCOMDATFolding>
+      <OptimizeReferences>true</OptimizeReferences>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+      <EnableUAC>false</EnableUAC>
+      <AdditionalLibraryDirectories>..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
+      <AdditionalDependencies>opencv_world480.lib;baseAlgorithm.lib;%(AdditionalDependencies)</AdditionalDependencies>
+    </Link>
+  </ItemDefinitionGroup>
+  <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
+  <ImportGroup Label="ExtensionTargets">
+  </ImportGroup>
+</Project>
--- a/QRcode3Ddetection_test/QRcode3Ddetection_test.cpp
+++ b/QRcode3Ddetection_test/QRcode3Ddetection_test.cpp
@ -0,0 +1,745 @@
+// QRcode3Ddetection_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
+//
+
+#include <iostream>
+#include <fstream>
+#include <vector>
+#include <stdio.h>
+#include <VZNL_Types.h>
+#include "direct.h"
+#include <string>
+#include "WD_QRcode3Ddetection_Export.h"
+#include <opencv2/opencv.hpp>
+#include <Windows.h>
+
+typedef struct
+{
+	int r;
+	int g;
+	int b;
+}SG_color;
+
+void vzReadLaserScanPointFromFile_plyTxt(const char* fileName, std::vector< SVzNL3DPoint>& scanData, bool removeZeros, bool exchangeXY)
+{
+	std::ifstream inputFile(fileName);
+	std::string linedata;
+
+	if (inputFile.is_open() == false)
+		return;
+
+	while (std::getline(inputFile, linedata))
+	{
+		if (linedata.empty())
+			continue;
+
+		double X, Y, Z, tmp;
+		sscanf_s(linedata.c_str(), "%lf %lf %lf %lf", &X, &Y, &Z, &tmp);
+		if (true == removeZeros)
+		{
+			if (Z > 1e-4)
+			{
+				SVzNL3DPoint a_pt;
+				if (true == exchangeXY)
+				{
+					a_pt.x = Y;  //将扫描线调整为Y方向；扫描方向为X方向
+					a_pt.y = X;
+				}
+				else
+				{
+					a_pt.x = X;
+					a_pt.y = Y;
+				}
+				a_pt.z = Z;
+				scanData.push_back(a_pt);
+			}
+		}
+		else
+		{
+			SVzNL3DPoint a_pt;
+			if (true == exchangeXY)
+			{
+				a_pt.x = Y;  //将扫描线调整为Y方向；扫描方向为X方向
+				a_pt.y = X;
+			}
+			else
+			{
+				a_pt.x = X;
+				a_pt.y = Y;
+			}
+			a_pt.z = Z;
+			scanData.push_back(a_pt);
+		}
+	}
+	return;
+}
+
+void _outputScanDataFile_vector(char* fileName, std::vector<std::vector<SVzNL3DPosition>>& scanLines, bool removeZeros, int* headNullLines)
+{
+	std::ofstream sw(fileName);
+	int lineNum = 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 = 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();
+}
+
+void _outputRGBDScanDataFile_RGBD(
+	char* fileName, 
+	std::vector<std::vector<SVzNL3DPosition>>& scanLines, 
+	std::vector< SVzNL3DPosition> objPoints)
+{
+	int lineNum = scanLines.size();
+	std::ofstream sw(fileName);
+	int realLines = lineNum;
+	if (objPoints.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 };
+
+	SG_color objColor[8] = {
+		{245,222,179},//淡黄色
+		{210,105, 30},//巧克力色
+		{240,230,140},//黄褐色
+		{135,206,235},//天蓝色
+		{250,235,215},//古董白
+		{189,252,201},//薄荷色
+		{221,160,221},//梅红色
+		{188,143,143},//玫瑰红色
+	};
+	int size = 1;
+	int lineIdx = 0;
+	for (int line = 0; line < lineNum; line++)
+	{
+		int linePtNum = scanLines[line].size();
+		if (linePtNum == 0)
+			continue;
+
+		sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
+		lineIdx++;
+		for (int i = 0; i < linePtNum; i++)
+		{
+			SVzNL3DPosition* pt3D = &scanLines[line][i];
+			int featureType = pt3D->nPointIdx;
+			if (LINE_FEATURE_PEAK_TOP == featureType)
+			{
+				rgb = { 255, 97, 0 };
+				size = 5;
+			}
+			else
+			{
+				rgb = { 200, 200, 200 };
+				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;
+		}
+	}
+	if (objPoints.size() > 0)
+	{
+		int linePtNum = objPoints.size();
+		sw << "Line_" << lineNum << "_0_" << linePtNum+1 << std::endl;
+
+		rgb = { 0, 0, 255 };
+		size = 25;
+		for (int i = 0; i < linePtNum; i++)
+		{
+			float x = (float)objPoints[i].pt3D.x;
+			float y = (float)objPoints[i].pt3D.y;
+			float z = (float)objPoints[i].pt3D.z;
+			sw << "{" << x << "," << y << "," << z << "}-";
+			sw << "{0,0}-{0,0}-";
+			sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
+		}
+		float x = (float)objPoints[0].pt3D.x;
+		float y = (float)objPoints[0].pt3D.y;
+		float z = (float)objPoints[0].pt3D.z;
+		sw << "{" << x << "," << y << "," << z << "}-";
+		sw << "{0,0}-{0,0}-";
+		sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
+	}
+	sw.close();
+}
+
+
+void _outputScanDataFile_ptr(char* fileName, SVzNL3DLaserLine* scanData, int lineNum)
+{
+	std::ofstream sw(fileName);
+	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++)
+	{
+		sw << "Line_" << line << "_" << scanData[line].nTimeStamp << "_" << scanData[line].nPositionCnt << std::endl;
+		for (int i = 0; i < scanData[line].nPositionCnt; i++)
+		{
+			SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i];
+			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}" << std::endl;
+		}
+	}
+	sw.close();
+}
+
+
+#define DATA_VER_OLD			0
+#define DATA_VER_NEW			1
+#define DATA_VER_FROM_CUSTOM	2
+#define VZ_LASER_LINE_PT_MAX_NUM 4096
+SVzNL3DLaserLine* vzReadLaserScanPointFromFile_XYZ(const char* fileName, int* scanLineNum, float* scanV,
+	int* dataCalib, int* scanMaxStamp, int* canClockUnit)
+{
+	std::ifstream inputFile(fileName);
+	std::string linedata;
+
+	if (inputFile.is_open() == false)
+		return NULL;
+
+	SVzNL3DLaserLine* _scanLines = NULL;
+
+	int lines = 0;
+	int dataElements = 4;
+	int firstIndex = -1;
+
+	int dataFileVer = DATA_VER_OLD;
+	std::getline(inputFile, linedata);  //第一行
+	int lineNum = 0;
+	if (0 == strncmp("LineNum:", linedata.c_str(), 8))
+	{
+		dataFileVer = DATA_VER_NEW;
+		sscanf_s(linedata.c_str(), "LineNum:%d", &lines);
+		if (lines == 0)
+			return NULL;
+		lineNum = lines;
+		_scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1));
+		memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1));
+		if (scanLineNum)
+			*scanLineNum = lines;
+	}
+	else if (0 == strncmp("LineNum_", linedata.c_str(), 8))
+	{
+		dataFileVer = DATA_VER_OLD;
+		sscanf_s(linedata.c_str(), "LineNum_%d", &lines);
+		if (lines == 0)
+			return NULL;
+		lineNum = lines;
+		_scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1));
+		memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1));
+		if (scanLineNum)
+			*scanLineNum = lines;
+	}
+	if (_scanLines == NULL)
+		return NULL;
+
+	int ptNum = 0;
+	int lineIdx = -1;
+	int ptIdx = 0;
+	SVzNL3DPosition* p3DPoint = NULL;
+	if (dataFileVer == DATA_VER_NEW)
+	{
+		while (getline(inputFile, linedata))
+		{
+			if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10))
+			{
+				double lineV = 0;
+				sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV);
+				if (scanV)
+					*scanV = (float)lineV;
+			}
+			else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12))
+			{
+				int ptAdjusted = 0;
+				sscanf_s(linedata.c_str(), "PointAdjust:%d", &ptAdjusted);
+				if (dataCalib)
+					*dataCalib = ptAdjusted;
+			}
+			else if (0 == strncmp("MaxTimeStamp:", linedata.c_str(), 13))
+			{
+				unsigned int maxTimeStamp = 0;
+				unsigned int timePerStamp = 0;
+				sscanf_s(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp);
+				if (scanMaxStamp)
+					*scanMaxStamp = maxTimeStamp;
+				if (canClockUnit)
+					*canClockUnit = timePerStamp;
+			}
+			else if (0 == strncmp("Line_", linedata.c_str(), 5))
+			{
+				int lineIndex;
+				unsigned int timeStamp;
+				sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum);
+				if (firstIndex < 0)
+					firstIndex = lineIndex;
+
+				lineIndex = lineIndex - firstIndex;
+				if ((lineIndex < 0) || (lineIndex >= lines))
+					break;
+
+				//new Line
+				lineIdx++;
+				if (ptNum > 0)
+				{
+					p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * ptNum);
+					memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum);
+				}
+				else
+					p3DPoint = NULL;
+				_scanLines[lineIdx].nPositionCnt = 0;
+				_scanLines[lineIdx].nTimeStamp = timeStamp;
+				_scanLines[lineIdx].p3DPosition = p3DPoint;
+
+			}
+			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);
+				int id = _scanLines[lineIdx].nPositionCnt;
+				if (id < ptNum)
+				{
+					p3DPoint[id].pt3D.x = X;
+					p3DPoint[id].pt3D.y = Y;
+					p3DPoint[id].pt3D.z = Z;
+					_scanLines[lineIdx].nPositionCnt = id + 1;
+				}
+			}
+		}
+
+	}
+	else if (dataFileVer == DATA_VER_OLD)
+	{
+		while (getline(inputFile, linedata))
+		{
+			if (0 == strncmp("DataElements_", linedata.c_str(), 13))
+			{
+				sscanf_s(linedata.c_str(), "DataElements_%d", &dataElements);
+				if ((dataElements != 3) && (dataElements != 4))
+					break;
+			}
+			if (0 == strncmp("LineV_", linedata.c_str(), 6))
+			{
+				double lineV = 0;
+				sscanf_s(linedata.c_str(), "LineV_%lf", &lineV);
+			}
+			else if (0 == strncmp("Line_", linedata.c_str(), 5))
+			{
+				int lineIndex;
+				unsigned int timeStamp;
+				sscanf_s(linedata.c_str(), "Line_%d_%u", &lineIndex, &timeStamp);
+#if 0
+				if (scanLineListTail == NULL)
+					firstIndex = lineIndex;
+#endif
+				lineIndex = lineIndex - firstIndex;
+				if ((lineIndex < 0) || (lineIndex >= lines))
+					break;
+				//new Line
+				//new Line
+				lineIdx++;
+				p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM);
+				memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM);
+				_scanLines[lineIdx].nPositionCnt = 0;
+				_scanLines[lineIdx].nTimeStamp = timeStamp;
+				_scanLines[lineIdx].p3DPosition = p3DPoint;
+			}
+			else if (0 == strncmp("(", linedata.c_str(), 1))
+			{
+				float X, Y, Z;
+				int imageY = 0;
+				if (dataElements == 4)
+					sscanf_s(linedata.c_str(), "(%f,%f,%f,%d)", &X, &Y, &Z, &imageY);
+				else
+					sscanf_s(linedata.c_str(), "(%f,%f,%f)", &X, &Y, &Z);
+				int id = _scanLines[lineIdx].nPositionCnt;
+				if (id < VZ_LASER_LINE_PT_MAX_NUM)
+				{
+					p3DPoint[id].pt3D.x = X;
+					p3DPoint[id].pt3D.y = Y;
+					p3DPoint[id].pt3D.z = Z;
+					_scanLines[lineIdx].nPositionCnt = id + 1;
+				}
+			}
+		}
+	}
+	inputFile.close();
+	return _scanLines;
+}
+
+void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector< SVzNL3DPoint>& scanData)
+{
+	std::ifstream inputFile(fileName);
+	std::string linedata;
+
+	if (inputFile.is_open() == false)
+		return;
+
+	while (getline(inputFile, linedata))
+	{
+		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);
+			SVzNL3DPoint a_pt;
+			a_pt.x = X;
+			a_pt.y = Y;
+			a_pt.z = Z;
+			scanData.push_back(a_pt);
+		}
+	}
+	inputFile.close();
+	return;
+}
+
+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();
+}
+
+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 _getRoiClouds(
+	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
+	int startLine,
+	int endLine,
+	int startPtIdx,
+	int endPtIdx,
+	std::vector< std::vector<SVzNL3DPosition>>& roiScanLines)
+{
+	for (int i = startLine; i < endLine; i++)
+	{
+		if (i >= scanLines.size())
+			break;
+
+		std::vector<SVzNL3DPosition> cut_line;
+		std::vector<SVzNL3DPosition>& a_line = scanLines[i];
+		for (int j = startPtIdx; j < endPtIdx; j++)
+		{
+			SVzNL3DPosition a_pt;
+			if (j >= a_line.size())
+			{
+				a_pt.nPointIdx = 0;
+				a_pt.pt3D = { 0,0,0 };
+			}
+			else
+				a_pt = a_line[j];
+			cut_line.push_back(a_pt);
+		}
+		roiScanLines.push_back(cut_line);
+	}
+	return;
+}
+
+#define TEST_COMPUTE_CALIB_PARA		0
+#define TEST_COMPUTE_QRCODE_IMG		1
+#define TEST_GROUP 1
+int main()
+{
+	const char* dataPath[TEST_GROUP] = {
+		"F:\\ShangGu\\项目\\工件端部圆点二维码\\",  //0
+
+	};
+
+	SVzNLRange fileIdx[TEST_GROUP] = {
+		{3,3}
+	};
+
+#if TEST_COMPUTE_CALIB_PARA
+	char _calib_datafile[256];
+	sprintf_s(_calib_datafile, "F:\\ShangGu\\项目\\工件端部圆点二维码\\LaserLine3_grid.txt");
+	int lineNum = 0;
+	float lineV = 0.0f;
+	int dataCalib = 0;
+	int maxTimeStamp = 0;
+	int clockPerSecond = 0;
+	SVzNL3DLaserLine* laser3DPoints = vzReadLaserScanPointFromFile_XYZ(_calib_datafile, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond);
+	if (laser3DPoints)
+	{
+		SSG_planeCalibPara calibPara = wd_getBaseCalibPara(
+			laser3DPoints,
+			lineNum);
+		//结果进行验证  
+		for (int i = 0; i < lineNum; i++)
+		{
+			if (i == 14)
+				int kkk = 1;
+			//行处理
+			//调平，去除地面
+			wd_lineDataR(&laser3DPoints[i], calibPara.planeCalib, -1);// calibPara.planeHeight);
+		}
+		//
+		char calibFile[250];
+		sprintf_s(calibFile, "F:\\ShangGu\\项目\\工件端部圆点二维码\\ground_calib_para.txt");
+		_outputCalibPara(calibFile, calibPara);
+		char _out_file[256];
+		sprintf_s(_out_file, "F:\\ShangGu\\项目\\工件端部圆点二维码\\LaserLine3_calib_data.txt");
+		_outputScanDataFile_ptr(_out_file, laser3DPoints, lineNum);
+		printf("%s: calib done!\n", _calib_datafile);
+	}
+
+#endif
+
+#if TEST_COMPUTE_QRCODE_IMG
+	for (int grp = 0; grp <= 0; grp++)
+	{
+		int cloud_rows = 4200;
+		int cloud_cols = 2160;
+		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 = -1.0;
+		for (int i = 0; i < 9; i++)
+			poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i];
+		char calibFile[250];
+		sprintf_s(calibFile, "F:\\ShangGu\\项目\\工件端部圆点二维码\\ground_calib_para.txt");
+		poseCalibPara = _readCalibPara(calibFile);
+
+		for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
+		{
+			//fidx = 1;
+			char _scan_file[256];
+			sprintf_s(_scan_file, "%s%d_Cloud.txt", dataPath[grp], fidx);
+			std::vector< SVzNL3DPoint> scanData;
+			bool removeZeros = false;
+			bool exchangeXY = true;
+			if (fidx < 3)
+			{
+				cloud_cols = 1;  //单行测试文件
+				exchangeXY = false;
+			}
+			else
+			{
+				cloud_cols = 2160;  //单行测试文件
+				exchangeXY = true;
+			}
+			if (fidx == 2)
+			{
+				vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanData); 
+				cloud_cols = 1100;
+				cloud_rows = 1300;
+				if (scanData.size() != (cloud_rows * cloud_cols))
+					continue;
+			}
+			else
+			{
+				vzReadLaserScanPointFromFile_plyTxt(_scan_file, scanData, removeZeros, exchangeXY);
+				if (scanData.size() != (cloud_rows * cloud_cols))
+					continue;
+			}
+			//将数据恢复为按扫描线存储格式
+			std::vector< std::vector<SVzNL3DPosition>> scanLines;
+			wd_getScanLines(scanData, scanLines, cloud_rows);
+
+#if 0
+			//点云裁剪
+			std::vector< std::vector<SVzNL3DPosition>> roiScanLines;
+			_getRoiClouds(scanLines, 700, 1800, 400, 1700, roiScanLines);
+			sprintf_s(_scan_file, "%sLaserLine4_grid.txt", dataPath[grp]);
+			int headNullLines = 0;
+			_outputScanDataFile_vector(_scan_file, roiScanLines, false, &headNullLines);
+#endif
+#if 0
+			sprintf_s(_scan_file, "%sLaserLine%d_grid.txt", dataPath[grp], fidx);
+			int headNullLines = 0;
+			_outputScanDataFile_vector(_scan_file, scanLines, false, &headNullLines);
+			printf("%s: head null lines = %d\n", _scan_file, headNullLines);
+#endif
+			WD_QRcodeParam qrcode_param;
+			qrcode_param.rows = 14;
+			qrcode_param.cols = 14;
+			qrcode_param.row_space = 30.0 / 14;
+			qrcode_param.col_space = 30.0 / 14;
+			qrcode_param.pointHoleDepth = 1.0;
+			qrcode_param.pointHoleR = 0.6;
+
+			long t1 = GetTickCount64();//统计时间
+
+			for (int i = 0, i_max= scanLines.size(); i < i_max; i++)
+			{
+				if (i == 14)
+					int kkk = 1;
+				//行处理
+				//调平，去除地面
+				wd_lineDataR(scanLines[i], poseCalibPara.planeCalib, -1);
+			}
+			//
+#if 0
+			sprintf_s(_scan_file, "%sresult\\LaserLine%d_calib_data.txt", dataPath[grp], fidx);
+			int headNulls = 0;
+			_outputScanDataFile_vector(_scan_file, scanLines, true, &headNulls);
+#endif
+			cv::Mat dmCodeImg;
+			std::vector< SVzNL3DPosition> objPoints;
+			wd_QRcode3Ddetection(
+				scanLines,
+				qrcode_param,
+				objPoints,
+				dmCodeImg);
+
+			long t2 = GetTickCount64();
+			printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
+			//输出测试结果
+			sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
+			_outputRGBDScanDataFile_RGBD(_scan_file, scanLines, objPoints);
+			if (!dmCodeImg.empty())
+			{
+				sprintf_s(_scan_file, "%sresult\\LaserLine%d_img.png", dataPath[grp], fidx);
+				cv::Mat normDmCodeImg;
+				cv::normalize(dmCodeImg, normDmCodeImg, 0, 255, cv::NORM_MINMAX, CV_8U);
+				cv::imwrite(_scan_file, normDmCodeImg);
+			}
+		}
+	}
+#endif
+}
+
+// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
+// 调试程序: F5 或调试 >“开始调试”菜单
+
+// 入门使用技巧: 
+//   1. 使用解决方案资源管理器窗口添加/管理文件
+//   2. 使用团队资源管理器窗口连接到源代码管理
+//   3. 使用输出窗口查看生成输出和其他消息
+//   4. 使用错误列表窗口查看错误
+//   5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
+//   6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件
--- a/QRcode3Ddetection_test/QRcode3Ddetection_test.vcxproj
+++ b/QRcode3Ddetection_test/QRcode3Ddetection_test.vcxproj
@ -0,0 +1,157 @@
+<?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>{e5faaacf-703c-4460-93b8-301991f62778}</ProjectGuid>
+    <RootNamespace>QRcode3Ddetectiontest</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>
+    <IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
+    <OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
+    <LinkIncremental>false</LinkIncremental>
+    <IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
+    <OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
+  </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\opencv\build\include;</AdditionalIncludeDirectories>
+    </ClCompile>
+    <Link>
+      <SubSystem>Console</SubSystem>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+      <AdditionalLibraryDirectories>..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
+      <AdditionalDependencies>opencv_world480d.lib;QRcode3Ddetection.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;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+      <AdditionalIncludeDirectories>..\..\thirdParty\opencv\build\include;</AdditionalIncludeDirectories>
+    </ClCompile>
+    <Link>
+      <SubSystem>Console</SubSystem>
+      <EnableCOMDATFolding>true</EnableCOMDATFolding>
+      <OptimizeReferences>true</OptimizeReferences>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+      <AdditionalLibraryDirectories>..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
+      <AdditionalDependencies>opencv_world480.lib;QRcode3Ddetection.lib;%(AdditionalDependencies)</AdditionalDependencies>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemGroup>
+    <ClCompile Include="QRcode3Ddetection_test.cpp" />
+  </ItemGroup>
+  <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
+  <ImportGroup Label="ExtensionTargets">
+  </ImportGroup>
+</Project>
--- a/SG_Algorithm.sln
+++ b/SG_Algorithm.sln
@ -66,6 +66,16 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "SieveNodeDetection_test", "
 		{D47DE411-B18C-4A64-BDB9-0AFAB50E8A8A} = {D47DE411-B18C-4A64-BDB9-0AFAB50E8A8A}
 	EndProjectSection
 EndProject
+Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "QRcode3Ddetection", "QRcode3Ddetection\QRcode3Ddetection.vcxproj", "{A7C72ED9-B81D-4C6A-BE38-D75199C824AA}"
+	ProjectSection(ProjectDependencies) = postProject
+		{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
+	EndProjectSection
+EndProject
+Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "QRcode3Ddetection_test", "QRcode3Ddetection_test\QRcode3Ddetection_test.vcxproj", "{E5FAAACF-703C-4460-93B8-301991F62778}"
+	ProjectSection(ProjectDependencies) = postProject
+		{A7C72ED9-B81D-4C6A-BE38-D75199C824AA} = {A7C72ED9-B81D-4C6A-BE38-D75199C824AA}
+	EndProjectSection
+EndProject
 Global
 	GlobalSection(SolutionConfigurationPlatforms) = preSolution
 		Debug|x64 = Debug|x64
@ -178,6 +188,22 @@ Global
 		{DF0881C7-43D0-4765-97CA-74C126665F3E}.Release|x64.Build.0 = Release|x64
 		{DF0881C7-43D0-4765-97CA-74C126665F3E}.Release|x86.ActiveCfg = Release|Win32
 		{DF0881C7-43D0-4765-97CA-74C126665F3E}.Release|x86.Build.0 = Release|Win32
+		{A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Debug|x64.ActiveCfg = Debug|x64
+		{A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Debug|x64.Build.0 = Debug|x64
+		{A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Debug|x86.ActiveCfg = Debug|Win32
+		{A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Debug|x86.Build.0 = Debug|Win32
+		{A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Release|x64.ActiveCfg = Release|x64
+		{A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Release|x64.Build.0 = Release|x64
+		{A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Release|x86.ActiveCfg = Release|Win32
+		{A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Release|x86.Build.0 = Release|Win32
+		{E5FAAACF-703C-4460-93B8-301991F62778}.Debug|x64.ActiveCfg = Debug|x64
+		{E5FAAACF-703C-4460-93B8-301991F62778}.Debug|x64.Build.0 = Debug|x64
+		{E5FAAACF-703C-4460-93B8-301991F62778}.Debug|x86.ActiveCfg = Debug|Win32
+		{E5FAAACF-703C-4460-93B8-301991F62778}.Debug|x86.Build.0 = Debug|Win32
+		{E5FAAACF-703C-4460-93B8-301991F62778}.Release|x64.ActiveCfg = Release|x64
+		{E5FAAACF-703C-4460-93B8-301991F62778}.Release|x64.Build.0 = Release|x64
+		{E5FAAACF-703C-4460-93B8-301991F62778}.Release|x86.ActiveCfg = Release|Win32
+		{E5FAAACF-703C-4460-93B8-301991F62778}.Release|x86.Build.0 = Release|Win32
 	EndGlobalSection
 	GlobalSection(SolutionProperties) = preSolution
 		HideSolutionNode = FALSE
--- a/SieveNodeDetection_test/SieveNodeDetection_test.cpp
+++ b/SieveNodeDetection_test/SieveNodeDetection_test.cpp
@ -484,7 +484,7 @@ typedef struct
 	int b;
 }SG_color;
 void _outputScanDataFile_RGBD_obj_XYZ(char* fileName, SVzNL3DLaserLine* scanData, int lineNum,
-	float lineV, int maxTimeStamp, int clockPerSecond, std::vector<SVzNL3DPoint>& objOps)
+	float lineV, int maxTimeStamp, int clockPerSecond, std::vector<std::vector<SVzNL3DPoint>>& objOps)
 {
 	std::ofstream sw(fileName);
 	int realLines = lineNum;
@ -605,35 +605,46 @@ void _outputScanDataFile_RGBD_obj_XYZ(char* fileName, SVzNL3DLaserLine* scanData
 	}
 	if (objOps.size() > 0)
 	{
-		int ptNum = objOps.size();
-		sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << ptNum << std::endl;
-		for (int i = 0; i < objOps.size(); i++)
+		int objNum = 0;
+		for (int i = 0, i_max = objOps.size(); i < i_max; i++)
 		{
-			if (i == 0)
-				rgb = { 255, 0, 0 };
-			else
-				rgb = { 255, 255, 0 };
-			size = 25;
-			float x = (float)objOps[i].x;
-			float y = (float)objOps[i].y;
-			float z = (float)objOps[i].z;
+			std::vector<SVzNL3DPoint>& obj_line = objOps[i];
+			objNum += obj_line.size();
+		}

-			sw << "{" << x << "," << y << "," << z << "}-";
-			sw << "{0,0}-{0,0}-";
-			sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
-			if (i == 0)
+		sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << objNum << std::endl;
+		for (int i = 0, i_max = objOps.size(); i < i_max; i++)
+		{
+			std::vector<SVzNL3DPoint>& obj_line = objOps[i];
+			for (int j = 0, j_max = obj_line.size(); j < j_max; j++)
 			{
+				rgb = objColor[i%8];
+				size = 25;
+				float x = (float)obj_line[j].x;
+				float y = (float)obj_line[j].y;
+				float z = (float)obj_line[j].z;
+
 				sw << "{" << x << "," << y << "," << z << "}-";
 				sw << "{0,0}-{0,0}-";
 				sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
+				if (i == 0)
+				{
+					sw << "{" << x << "," << y << "," << z << "}-";
+					sw << "{0,0}-{0,0}-";
+					sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
+				}
 			}
 		}
 	}
 	sw.close();
 }

-void _XOYprojection_nodePos(cv::Mat& img, std::vector<std::vector< SVzNL3DPosition>>& dataLines, std::vector<SVzNL3DPoint>& objOps,
-	const double x_scale, const double y_scale, const SVzNLRangeD x_range, const SVzNLRangeD y_range)
+void _XOYprojection_nodePos(
+	cv::Mat& img, 
+	std::vector<std::vector< SVzNL3DPosition>>& dataLines, 
+	std::vector<std::vector<SVzNL3DPoint>>& objOps,
+	const double x_scale, const double y_scale, 
+	const SVzNLRangeD x_range, const SVzNLRangeD y_range)
 {
 	int x_skip = 16;
 	int y_skip = 16;
@ -748,27 +759,24 @@ void _XOYprojection_nodePos(cv::Mat& img, std::vector<std::vector< SVzNL3DPositi
 	}
 	if (objOps.size() > 0)
 	{
-		for (int i = 0; i < objOps.size(); i++)
+		for (int i = 0, i_max = objOps.size(); i < i_max; i++)
 		{
-			if (i == 0)
+			std::vector<SVzNL3DPoint>& obj_line = objOps[i];
+			for (int j = 0, j_max = obj_line.size(); j < j_max; j++)
 			{
-				rgb = { 255, 0, 0 };
+				rgb = objColor[i % 8];
 				size = 20;
+
+				int px = (int)((obj_line[j].x - x_range.min) / x_scale + x_skip);
+				int py = (int)((obj_line[j].y - y_range.min) / y_scale + y_skip);
+				cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1);
 			}
-			else
-			{
-				rgb = { 255, 255, 0 };
-				size = 10;
-			}
-			int px = (int)((objOps[i].x - x_range.min) / x_scale + x_skip);
-			int py = (int)((objOps[i].y - y_range.min) / y_scale + y_skip);
-			cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1);
 		}
 	}

 }

-void _genXOYProjectionImage_nodePos(cv::String& fileName, SVzNL3DLaserLine* scanData, int lineNum, std::vector<SVzNL3DPoint>& nodeOps)
+void _genXOYProjectionImage_nodePos(cv::String& fileName, SVzNL3DLaserLine* scanData, int lineNum, std::vector<std::vector<SVzNL3DPoint>>& nodeOps)
 {
 	//统计X和Y的范围
 	std::vector<std::vector< SVzNL3DPosition>> scan_lines;
@ -911,12 +919,160 @@ SSG_planeCalibPara _readCalibPara(char* fileName)
 	return planePara;
 }

+std::vector<SVzNL3DPoint>  _wdReadLMIData_TXT(char* fileName)
+{
+	std::ifstream inputFile(fileName);
+	std::string linedata;
+
+	std::vector<SVzNL3DPoint> resultData;
+	if (inputFile.is_open() == false)
+		return resultData;
+
+	while (getline(inputFile, linedata))
+	{
+		double x, y, z;
+		sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &y, &x, &z);
+		SVzNL3DPoint a_pt;
+		a_pt.x = x;
+		a_pt.y = y;
+		a_pt.z = z;
+		resultData.push_back(a_pt);
+	}
+	return resultData;
+}
+
+std::vector<SVzNL3DPoint>  _wdReadGuangZiData_TXT(char* fileName, double tick_scale, double offset)
+{
+	std::ifstream inputFile(fileName);
+	std::string linedata;
+
+	std::vector<SVzNL3DPoint> resultData;
+	if (inputFile.is_open() == false)
+		return resultData;
+
+	while (getline(inputFile, linedata))
+	{
+		double x, y, z;
+		sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &x, &y, &z);
+		if (z > -9999.0 + 1e-4) //1e-4为防止float误差
+		{
+			SVzNL3DPoint a_pt;
+			a_pt.x = (x - offset)* tick_scale;
+			a_pt.y = y;
+			a_pt.z = z;
+			resultData.push_back(a_pt);
+		}
+	}
+	return resultData;
+}
+
+SVzNL3DLaserLine* _convertToVizumDataFormat(std::vector<SVzNL3DPoint>& ptList, int* vldLineNum)
+{
+	
+	std::vector<std::vector< SVzNL3DPoint>> lineList;
+	std::vector< SVzNL3DPoint> a_line;
+	double pre_y = 1.0e+10;;
+	int maxLineSize = 0;
+	for (int i = 0, i_max = ptList.size(); i < i_max; i++)
+	{
+		if (ptList[i].y < pre_y-1.0) //1.0为容错门限，防止相邻点有反转
+		{
+			//新行开始
+			if (a_line.size() > 0)
+			{
+				lineList.push_back(a_line);
+				if (maxLineSize < a_line.size())
+					maxLineSize = a_line.size();
+				a_line.clear();
+			}
+			a_line.push_back(ptList[i]);
+		}
+		else
+			a_line.push_back(ptList[i]);
+		pre_y = ptList[i].y;
+	}
+	//最后一行
+	if (a_line.size() > 0)
+	{
+		lineList.push_back(a_line);
+		if (maxLineSize < a_line.size())
+			maxLineSize = a_line.size();
+	}
+
+	int lineNum = lineList.size();
+	*vldLineNum = lineNum;
+	SVzNL3DLaserLine* resultdData = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * lineNum);
+	memset(resultdData, 0, sizeof(SVzNL3DLaserLine) * lineNum);
+	for (int line = 0; line < lineNum; line++)
+	{
+		std::vector< SVzNL3DPoint>& a_lineData = lineList[line];
+		int pt_counter = a_lineData.size();
+		resultdData[line].nPositionCnt = pt_counter;
+		resultdData[line].nTimeStamp = 0;
+		resultdData[line].p3DPosition = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * pt_counter);
+		memset(resultdData[line].p3DPosition, 0, sizeof(SVzNL3DPosition) * pt_counter);
+		for (int i = 0; i < pt_counter; i++)
+		{
+			resultdData[line].p3DPosition[i].nPointIdx = i;
+			resultdData[line].p3DPosition[i].pt3D = a_lineData[i];
+		}
+	}
+	return resultdData;
+}
+
+
 #define TEST_CONVERT_TO_GRID		0
 #define TEST_COMPUTE_SIEVE_NODES	1
 #define TEST_COMPUTE_CALIB_PARA		0
+#define TEST_CONVERT_LMI_DATA		0
+#define TEST_CONVERT_GUANGZI_DATA		0
 #define TEST_GROUP 2
 int main()
 {
+#if TEST_CONVERT_LMI_DATA
+	//将数据转换成栅格格式格式
+	int lineNum = 0;
+	float lineV = 0.0f;
+	int dataCalib = 0;
+	int maxTimeStamp = 0;
+	int clockPerSecond = 0;
+	char _LMI_file[256];
+	sprintf_s(_LMI_file, "F:\\张奔-标定采图\\钢块-标定采图\\5-右下-2.txt");
+	std::vector<SVzNL3DPoint>  LMI_data = _wdReadLMIData_TXT(_LMI_file);
+	if (LMI_data.size() == 0)
+		return 0;
+
+	int vldLineNum = 0;
+	SVzNL3DLaserLine* convertData = _convertToVizumDataFormat(LMI_data, &vldLineNum);
+	char _out_file[256];
+	sprintf_s(_out_file, "F:\\张奔-标定采图\\钢块-标定采图\\5-右下-2_onverted.txt");
+	_outputScanDataFile_self(_out_file, convertData, vldLineNum,
+		lineV, maxTimeStamp, clockPerSecond);
+	printf("%s: convert done!\n", _LMI_file);
+#endif
+#if TEST_CONVERT_GUANGZI_DATA
+	//将数据转换成栅格格式格式
+	int lineNum = 0;
+	float lineV = 0.0f;
+	int dataCalib = 0;
+	int maxTimeStamp = 0;
+	int clockPerSecond = 0;
+	char _Guangzi_file[256];
+	double offset = 300000;
+	double tick_scale = 0.05; //左正
+	sprintf_s(_Guangzi_file, "F:\\张奔-标定采图\\钢块-标定采图\\5-右正-2.txt");
+	std::vector<SVzNL3DPoint>  Guangzi_data = _wdReadGuangZiData_TXT(_Guangzi_file, tick_scale, offset);
+	if (Guangzi_data.size() == 0)
+		return 0;
+
+	int vldLineNum = 0;
+	SVzNL3DLaserLine* convertData = _convertToVizumDataFormat(Guangzi_data, &vldLineNum);
+	char _out_file[256];
+	sprintf_s(_out_file, "F:\\张奔-标定采图\\钢块-标定采图\\5-右正-2_onverted.txt");
+	_outputScanDataFile_self(_out_file, convertData, vldLineNum,
+		lineV, maxTimeStamp, clockPerSecond);
+	printf("%s: convert done!\n", _Guangzi_file);
+#endif
 #if TEST_CONVERT_TO_GRID
 	//将数据转换成栅格格式格式
 	char _scan_dir[256];
@ -1111,7 +1267,7 @@ int main()
 				sg_lineDataR(&laser3DPoints[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight);
 			}
 			//计算节点位置
-			std::vector<SVzNL3DPoint> nodePos;
+			std::vector<std::vector<SVzNL3DPoint>> nodePos;
 			sg_getSieveNodes(
 				laser3DPoints,
 				lineNum,
--- a/bagPositioning_test/bagPositioning_test.cpp
+++ b/bagPositioning_test/bagPositioning_test.cpp
--- a/baseAlgorithm/baseAlgorithm.vcxproj
+++ b/baseAlgorithm/baseAlgorithm.vcxproj
@ -166,6 +166,7 @@
  </ItemGroup>
  <ItemGroup>
    <ClCompile Include="..\sourceCode\SG_baseFunc.cpp" />
+    <ClCompile Include="..\sourceCode\SG_clustering.cpp" />
    <ClCompile Include="..\sourceCode\SG_featureGrow.cpp" />
    <ClCompile Include="..\sourceCode\SG_lineFeature.cpp" />
    <ClCompile Include="..\sourceCode\SG_regionGrow.cpp" />
--- a/sourceCode/SG_bagPositioning.cpp
+++ b/sourceCode/SG_bagPositioning.cpp
@ -12,6 +12,13 @@ void sg_lineDataR(SVzNL3DLaserLine* a_line,
 	lineDataRT(	a_line,	camPoseR, groundH);
 }

+void sg_lineDataR_RGBD(SVzNLXYZRGBDLaserLine* a_line,
+	const double* camPoseR,
+	double groundH)
+{
+	lineDataRT_RGBD(a_line, camPoseR, groundH);
+}
+
 //扫描线处理，进行垂直方向的特征提取和生长
 void sg_bagPositioning_lineProc(
 	SVzNL3DLaserLine* a_line, 
@ -2289,7 +2296,7 @@ void sg_getBagPosition(
 	cv::imwrite("originMask.png", oriMaskImage);
 #endif

-	int vldRgnSize = (int)((algoParam.bagParam.bagL / 2) * (algoParam.bagParam.bagW / 2) / (scale * scale));
+	int vldRgnSize = (int)((algoParam.bagParam.bagL * algoParam.bagParam.bagW / 2) / (scale * scale));
 	for (int rgnid = 0, rgn_max = labelRgns.size(); rgnid < rgn_max; rgnid++)
 	{
 		SSG_Region* a_rgn = &labelRgns[rgnid];
@ -3807,6 +3814,440 @@ if (i == 2)
 	}
 }

+typedef struct
+{
+	int vldNum[RGN_HIST_SIZE];
+	int totalNum[RGN_HIST_SIZE];
+}_rgnHSVHistInfo;
+
+HSV RGBtoHSV(int r, int g, int b) {
+	double r_ = r / 255.0;
+	double g_ = g / 255.0;
+	double b_ = b / 255.0;
+	double max_ = std::max(r_, std::max(g_, b_));
+	double min_ = std::min(r_, std::min(g_, b_));
+	double delta = max_ - min_;
+	double h, s, v;
+	v = max_;
+	s = (max_ == 0) ? 0 : (delta / max_);
+	if (delta == 0) //此处有修改。设定一个门限
+	{
+		h = -1; // 未定义，可以是任何值，通常设为0或NaN
+	}
+	else 
+	{
+		if (r_ == max_) {
+			h = (g_ - b_) / delta;
+		}
+		else if (g_ == max_) {
+			h = 2 + (b_ - r_) / delta;
+		}
+		else { // b_ == max_
+			h = 4 + (r_ - g_) / delta;
+		}
+		h *= 60; // 将h的值标准化到[0,360]度
+		if (h < 0) {
+			h += 360; // 确保h在[0,360]范围内
+		}
+	}
+	HSV hsv;
+	hsv.h = h;
+	hsv.s = s * 255;// 将s的值标准化到[0,255]
+	hsv.v = v * 255;// 将v的值标准化到[0,255]
+	return hsv;
+}
+
+bool isSameColor(HSV hsvPattern, HSV hsvPt, const SSG_hsvCmpParam colorCmpParam)
+{
+	if (((hsvPattern.h < 0) && (hsvPt.h >= 0)) || ((hsvPattern.h >= 0) && (hsvPt.h < 0)))
+		return false;
+
+	if ((hsvPattern.h < 0) && (hsvPt.h < 0))
+		return true;
+
+	double h_diff = hsvPt.h - hsvPt.h;
+	if (h_diff < -180)
+		h_diff = h_diff + 360;
+	else if (h_diff > 180)
+		h_diff = h_diff - 360;
+	h_diff = abs(h_diff);
+	double s_diff = abs(hsvPattern.s - hsvPt.s);
+	if ( (h_diff < colorCmpParam.hueTh) && (s_diff < colorCmpParam.saturateTh))
+		return true;
+	else
+		return false;
+}
+
+double cosSimilarity(const double* colorTemplate, double* sampleData)
+{
+	double sumA = 0, sumB = 0, sumAB = 0;
+	for (int i = 0; i < RGN_HIST_SIZE; i++)
+	{
+		sumA += colorTemplate[i] * colorTemplate[i];
+		sumAB += colorTemplate[i] * sampleData[i];
+		sumB = sampleData[i] * sampleData[i];
+	}
+	sumA = sqrt(sumA);
+	sumB = sqrt(sumB);
+	sumA = sumA * sumB;
+	if (sumA <= 1e-4)
+		return 0;
+	return (sumAB / sumA);
+}
+
+double cosSimilarityInv(const double* colorTemplate, double* sampleData)
+{
+	double sumA = 0, sumB = 0, sumAB = 0;
+	for (int i = 0; i < RGN_HIST_SIZE; i++)
+	{
+		sumA += colorTemplate[i] * colorTemplate[i];
+		sumAB += colorTemplate[i] * sampleData[RGN_HIST_SIZE - 1 - i];
+		sumB = sampleData[RGN_HIST_SIZE - 1 - i] * sampleData[RGN_HIST_SIZE -1 - i];
+	}
+	sumA = sqrt(sumA);
+	sumB = sqrt(sumB);
+	sumA = sumA * sumB;
+	if (sumA <= 1e-4)
+		return 0;
+	return (sumAB / sumA);
+}
+
+///数据输入必须是RGBD grid格式，以进行水平方向和垂直方向的处理
+///（1）寻找边界点
+///（2）从最高点开始进行区域生长
+///  输出编织袋及袋子上下朝向
+void sg_getBagPositionAndOrientation(
+	SVzNLXYZRGBDLaserLine* laser3DPoints,
+	int lineNum,
+	//std::vector<SSG_lineFeature>& all_vLineFeatures, 
+	//std::vector<std::vector<int>>& noisePts, 
+	const SG_bagPositionParam algoParam,
+	const SSG_planeCalibPara poseCalibPara,
+	const SSG_hsvCmpParam colorCmpParam,
+	const RGB rgbColorPattern,  //用于区分袋子方向的颜色
+	const double frontColorTemplate[RGN_HIST_SIZE],
+	const double backColorTemplate[RGN_HIST_SIZE],
+	std::vector<SSG_peakOrienRgnInfo>& objOps,
+#if OUTPUT_DEBUG
+	std::vector<std::vector< SVzNL3DPosition>>& bagPositionCloudPts,
+#endif
+	int* errCode)
+{
+	*errCode = 0;
+	if (lineNum == 0)
+	{
+		*errCode = SG_ERR_3D_DATA_INVLD;
+		return;
+	}
+	//检查是否为栅格格式
+	//将RGBD点云分离出对应的RGB和点云，RGB转成HSV
+	std::vector<SVzNL3DLaserLine> cloudPts;
+	std::vector<std::vector<HSV>> hsvPts;
+	bool dataValid = true;
+	int linePtNum = laser3DPoints[0].nPointCnt;
+	for (int line = 0; line < lineNum; line++)
+	{
+		if (laser3DPoints[line].nPointCnt > 0)
+		{
+			std::vector<HSV> line_hsv;
+			line_hsv.resize(laser3DPoints[line].nPointCnt);
+			SVzNL3DPosition* p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * laser3DPoints[line].nPointCnt);
+			memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * laser3DPoints[line].nPointCnt);
+			for (int i = 0; i < laser3DPoints[line].nPointCnt; i++)
+			{
+				if ((line == 497) && (i >= 902))
+					int kkk = 1;
+				p3DPoint[i].pt3D.x = laser3DPoints[line].p3DPoint[i].x;
+				p3DPoint[i].pt3D.y = laser3DPoints[line].p3DPoint[i].y;
+				p3DPoint[i].pt3D.z = laser3DPoints[line].p3DPoint[i].z;
+				p3DPoint[i].nPointIdx = i;
+
+				unsigned int nRGB = laser3DPoints[line].p3DPoint[i].nRGB;
+
+				int r = nRGB & 0xff;
+				nRGB >>= 8;
+				int g = nRGB & 0xff;
+				nRGB >>= 8;
+				int b = nRGB & 0xff;
+				line_hsv[i] = RGBtoHSV(r,g,b);
+			}
+
+			SVzNL3DLaserLine a_line;
+			a_line.nPositionCnt = laser3DPoints[line].nPointCnt;
+			a_line.nTimeStamp = 0;
+			a_line.p3DPosition = p3DPoint;
+			cloudPts.push_back(a_line);
+			hsvPts.push_back(line_hsv);
+		}
+		if (linePtNum != laser3DPoints[line].nPointCnt)
+			dataValid = false;
+	}
+
+	if (false == dataValid)
+	{
+		*errCode = SG_ERR_3D_DATA_INVLD;
+		//释放内存
+		for (int i = 0, i_max = cloudPts.size(); i < i_max; i++)
+			free(cloudPts[i].p3DPosition);
+		return;
+	}
+
+	//根据目标
+	std::vector<SSG_peakRgnInfo> peakRgn;
+	sg_getBagPosition(
+		&cloudPts[0],
+		cloudPts.size(),
+		algoParam,
+		poseCalibPara,
+		peakRgn);
+
+	//释放内存
+	for (int i = 0, i_max = cloudPts.size(); i < i_max; i++)
+	{
+#if OUTPUT_DEBUG
+		std::vector< SVzNL3DPosition> out_line;
+		int nPtCounter = cloudPts[i].nPositionCnt;
+		for (int j = 0; j < nPtCounter; j++)
+			out_line.push_back(cloudPts[i].p3DPosition[j]);
+		bagPositionCloudPts.push_back(out_line);
+#endif
+		free(cloudPts[i].p3DPosition);
+		cloudPts[i].p3DPosition = NULL;
+	}
+
+	//将数据重新投射回原来的坐标系，以保持手眼标定结果正确
+	for (int i = 0; i < lineNum; i++)
+		sg_lineDataR_RGBD(&laser3DPoints[i], poseCalibPara.invRMatrix, -1);
+
+	//将颜色属性转成HSV
+	HSV hsvPattern = RGBtoHSV(rgbColorPattern.r, rgbColorPattern.g, rgbColorPattern.b);
+	//进行颜色统计处理：取centerPos为中心的（0.8*Width, 0.8*Height）的范围内的点进行统计
+	int objNum = peakRgn.size();
+	if (objNum > 0)
+	{
+		//以水平放置的矩阵为标准模型位置，即水平方向为长度，垂直方向为宽度
+		std::vector<double> y_th;
+		y_th.resize(objNum);
+		std::vector<double> x_th;
+		x_th.resize(objNum);
+		std::vector<double> x_slice;
+		x_slice.resize(objNum);
+		for (int m = 0; m < objNum; m++)
+		{
+			y_th[m] = peakRgn[m].objSize.dHeight * 0.25;
+			x_th[m] = peakRgn[m].objSize.dWidth * 0.45;
+			x_slice[m] = peakRgn[m].objSize.dWidth / 16.0;
+		}
+
+		std::vector<SSG_ROIRectD> rgnROIs;
+		rgnROIs.resize(objNum);
+		for (int i = 0; i < objNum; i++)
+		{
+			//计算一个大的ROI，加速处理
+			//以水平放置的矩阵为标准模型位置
+			SVzNL2DPointD vec[4];
+			vec[0].x = -peakRgn[i].objSize.dWidth / 2;
+			vec[0].y = -peakRgn[i].objSize.dHeight / 2;
+			vec[1].x =  peakRgn[i].objSize.dWidth / 2;
+			vec[1].y = -peakRgn[i].objSize.dHeight / 2;
+			vec[2].x =  peakRgn[i].objSize.dWidth / 2;
+			vec[2].y =  peakRgn[i].objSize.dHeight / 2;
+			vec[3].x = -peakRgn[i].objSize.dWidth / 2;
+			vec[3].y =  peakRgn[i].objSize.dHeight / 2;
+			//旋转
+			const double deg2rad = PI / 180.0;
+			const double yaw = peakRgn[i].centerPos.z_yaw * deg2rad;
+			double cy = cos(yaw); double sy = sin(yaw);
+			for (int m = 0; m < 4; m++)
+			{
+				double x = vec[m].x * cy + vec[m].y * sy;  //图像坐标系的Y与欧氏坐标系方向相反，
+				double y = -vec[m].x * sy + vec[m].y * cy;
+				vec[m].x = x + peakRgn[i].centerPos.x;
+				vec[m].y = y + peakRgn[i].centerPos.y;
+			}
+			//生成ROI
+			SSG_ROIRectD a_roi;
+			a_roi.left = vec[0].x;
+			a_roi.right = vec[0].x;
+			a_roi.top = vec[0].y;
+			a_roi.bottom = vec[0].y;
+			for (int m = 1; m < 4; m++)
+			{
+				a_roi.left =	a_roi.left   > vec[m].x ? vec[m].x : a_roi.left;
+				a_roi.right =	a_roi.right  < vec[m].x ? vec[m].x : a_roi.right;
+				a_roi.top =		a_roi.top    > vec[m].y ? vec[m].y : a_roi.top;
+				a_roi.bottom =	a_roi.bottom < vec[m].y ? vec[m].y : a_roi.bottom;
+			}
+			rgnROIs[i] = a_roi;
+		}
+
+		std::vector<_rgnHSVHistInfo> rgnHists; //统计量
+		rgnHists.resize(objNum);
+		//统计量清零
+		for (int m = 0; m < objNum; m++)
+		{
+			for (int j = 0; j < 16; j++)
+			{
+				rgnHists[m].totalNum[j] = 0;
+				rgnHists[m].vldNum[j] = 0;
+			}
+		}
+
+		//对所有点进投射，注意此处使用原始点
+		for (int line = 0; line < lineNum; line++)
+		{
+			for (int j = 0; j < linePtNum; j++)
+			{
+				SVzNLPointXYZRGBA* a_pt = &laser3DPoints[line].p3DPoint[j];
+				if (a_pt->z < 1e-4)
+					continue;
+				if ((line == 497) && (j >= 902))
+				{
+					HSV a_hsv = hsvPts[line][j];
+					int kkk = 1;
+				}
+				for (int m = 0; m < objNum; m++)
+				{
+					//检查点是否在ROI内
+					if ((a_pt->x >= rgnROIs[m].left) && (a_pt->x <= rgnROIs[m].right) &&
+						(a_pt->y >= rgnROIs[m].top) && (a_pt->y <= rgnROIs[m].bottom))
+					{
+						//计算投射点
+						//反向旋转,逆时针角度
+						const double deg2rad = PI / 180.0;
+						double rotateAngle = - peakRgn[m].centerPos.z_yaw * deg2rad;
+						double cy = cos(rotateAngle); double sy = sin(rotateAngle);
+						double x = a_pt->x - peakRgn[m].centerPos.x;
+						double y = a_pt->y - peakRgn[m].centerPos.y;
+						double rx = x * cy + y * sy;
+						double ry = -x * sy + y * cy;
+						if ((rx >= -x_th[m]) && (rx <= x_th[m]) && (ry >= -y_th[m]) && (ry <= y_th[m]))
+						{
+							if (m == 0)
+								int kkk = 1;
+							else if (m == 1)
+								int kkk = 1;
+							else if (m == 2)
+								int kkk = 1;
+							else if (m == 3)
+								int kkk = 1;
+							double rx_abs = abs(rx);
+							int hist_idx = rx_abs / x_slice[m];
+							if (hist_idx >= RGN_HIST_SIZE / 2)
+								hist_idx = RGN_HIST_SIZE / 2 - 1;
+							if (rx < 0)
+								hist_idx = -hist_idx + RGN_HIST_SIZE / 2 - 1;
+							else
+								hist_idx = hist_idx + RGN_HIST_SIZE / 2;
+							//判断属性
+							bool isSame = isSameColor(hsvPattern, hsvPts[line][j], colorCmpParam);
+							if (true == isSame)
+								rgnHists[m].vldNum[hist_idx] ++;
+							rgnHists[m].totalNum[hist_idx] ++;
+						}
+					}
+				}
+			}
+		}
+
+
+		for (int m = 0; m < objNum; m++)
+		{
+			double normalizeHist[RGN_HIST_SIZE];
+			//生成归一化数据
+			int totalNum=0, totalVldNum=0, upVldNum=0, dwnVldNum=0;
+			for (int j = 0; j < RGN_HIST_SIZE; j++)
+			{
+				totalNum += rgnHists[m].totalNum[j];
+				totalVldNum += rgnHists[m].vldNum[j];
+
+				if (j < (RGN_HIST_SIZE / 2-2))
+					dwnVldNum += rgnHists[m].vldNum[j];
+				else if (j >= (RGN_HIST_SIZE / 2+2))
+					upVldNum += rgnHists[m].vldNum[j];
+
+				if (rgnHists[m].totalNum[j] > 0)
+					normalizeHist[j] = (double)rgnHists[m].vldNum[j] / (double)rgnHists[m].totalNum[j];
+				else
+					normalizeHist[j] = 0;
+			}
+
+			SSG_peakOrienRgnInfo a_obj;
+			a_obj.centerPos = peakRgn[m].centerPos;
+			a_obj.objSize = peakRgn[m].objSize;
+			a_obj.pkRgnIdx = peakRgn[m].pkRgnIdx;
+			a_obj.pos2D = peakRgn[m].pos2D;
+			a_obj.orienFlag = 0;  //未知正反
+			//相似度比较
+#if 0
+			double frontSim = cosSimilarity(frontColorTemplate, normalizeHist);
+			double frontSim_inv = cosSimilarityInv(frontColorTemplate, normalizeHist);
+			double backSim = cosSimilarity(backColorTemplate, normalizeHist);
+			double backSim_inv = cosSimilarityInv(backColorTemplate, normalizeHist);
+			if ((frontSim > frontSim_inv) && (frontSim > backSim) && (frontSim > backSim_inv) && (frontSim > 0.5))
+			{
+				a_obj.orienFlag = 1;
+			}
+			else if ((frontSim_inv > frontSim) && (frontSim_inv > backSim) && (frontSim_inv > backSim_inv) && (frontSim_inv > 0.5))
+			{
+				a_obj.orienFlag = 1;
+				a_obj.centerPos.z_yaw += 180;
+				if (a_obj.centerPos.z_yaw >= 360)
+					a_obj.centerPos.z_yaw = a_obj.centerPos.z_yaw - 360;
+			}
+			else if ((backSim > frontSim) && (backSim > frontSim_inv) && (backSim > backSim_inv) && (backSim > 0.5))
+			{
+				a_obj.orienFlag = 2;
+			}
+			else if ((backSim_inv > frontSim) && (backSim_inv > frontSim_inv) && (backSim_inv > backSim) && (backSim_inv > 0.5))
+			{
+				a_obj.orienFlag = 2;
+				a_obj.centerPos.z_yaw += 180;
+				if (a_obj.centerPos.z_yaw >= 360)
+					a_obj.centerPos.z_yaw = a_obj.centerPos.z_yaw - 360;
+			}
+#else
+			double vldPtRatio = (double)totalVldNum / (double)totalNum;
+			bool isFront;
+			if (true == colorCmpParam.frontVldPtGreater)
+				isFront = vldPtRatio > colorCmpParam.FBVldPtRatioTh ? true : false;
+			else
+				isFront = vldPtRatio > colorCmpParam.FBVldPtRatioTh ? false : true;
+
+			bool orienIsNormal;
+			if (true == isFront)
+			{
+				if (true == colorCmpParam.front_upVldPtGreater)
+					orienIsNormal = upVldNum > dwnVldNum ? true : false;
+				else
+					orienIsNormal = upVldNum > dwnVldNum ? false : true;
+			}
+			else
+			{
+				if (true == colorCmpParam.back_upVldPtGreater)
+					orienIsNormal = upVldNum > dwnVldNum ? true : false;
+				else
+					orienIsNormal = upVldNum > dwnVldNum ? false : true;
+			}
+
+			if (true == isFront)
+				a_obj.orienFlag = 1;
+			else
+				a_obj.orienFlag = 2;
+
+			if (false == orienIsNormal)
+			{
+				a_obj.centerPos.z_yaw += 180;
+				if (a_obj.centerPos.z_yaw >= 360)
+					a_obj.centerPos.z_yaw = a_obj.centerPos.z_yaw - 360;
+			}
+#endif
+			objOps.push_back(a_obj);
+		}
+	}
+
+}
+
 bool compareByXPos(const SSG_featureTree& a, const SSG_featureTree& b) {
 	return (a.roi.left + a.roi.right) < (b.roi.left + b.roi.right);
 }
--- a/sourceCode/SG_bagPositioning_Export.h
+++ b/sourceCode/SG_bagPositioning_Export.h
@ -6,8 +6,9 @@
 #  define SG_APISHARED_EXPORT __declspec(dllimport)
 #endif

-#define OUTPUT_DEBUG	0
+#define OUTPUT_DEBUG	1
 #define BAG_ALGO_USE_CORNER_FEATURE	1
+#define RGN_HIST_SIZE	16 //目标颜色统计的数目

 #include "SG_baseDataType.h"
 #include <vector>
@ -27,6 +28,16 @@ typedef struct
 	//SSG_polarScanParam polarScanParam;
 }SG_bagPositionParam;

+typedef struct
+{
+	double hueTh;
+	double saturateTh;
+	double FBVldPtRatioTh; //正反两面有效颜色点的比例门限
+	bool frontVldPtGreater; //true：有效颜色比例高的点的是正面；false：有效颜色比例高的点的是反面
+	bool front_upVldPtGreater;//true：有效颜色比例高的点的是正面朝上；false：有效颜色比例高的点的是正面朝下
+	bool back_upVldPtGreater; //true：有效颜色比例高的点的是反面朝上；false：有效颜色比例高的点的是反面朝下
+}SSG_hsvCmpParam;
+
 typedef struct
 {
 	double jumpTh;  //和现场环境有关，为托盘的孔洞的Z跳变
@ -34,7 +45,13 @@ typedef struct
 	double baseHoleDist; //托盘两个孔洞的距离
 }SSG_stackBaseParam;

-SG_APISHARED_EXPORT void sg_lineDataR(SVzNL3DLaserLine* a_line,
+//数据调平
+SG_APISHARED_EXPORT void sg_lineDataR(
+	SVzNL3DLaserLine* a_line,
+	const double* camPoseR,
+	double groundH);
+SG_APISHARED_EXPORT void sg_lineDataR_RGBD(
+	SVzNLXYZRGBDLaserLine* a_line,
 	const double* camPoseR,
 	double groundH);

@ -56,6 +73,23 @@ SG_APISHARED_EXPORT void sg_getBagPosition(
 	const SSG_planeCalibPara poseCalibPara,
 	std::vector<SSG_peakRgnInfo>& objOps);

+SG_APISHARED_EXPORT void sg_getBagPositionAndOrientation(
+	SVzNLXYZRGBDLaserLine* laser3DPoints,
+	int lineNum,
+	//std::vector<SSG_lineFeature>& all_vLineFeatures, 
+	//std::vector<std::vector<int>>& noisePts, 
+	const SG_bagPositionParam algoParam,
+	const SSG_planeCalibPara poseCalibPara,
+	const SSG_hsvCmpParam colorCmpParam,
+	const RGB rgbColorPattern,  //用于区分袋子方向的颜色
+	const double frontColorTemplate[RGN_HIST_SIZE],
+	const double backColorTemplate[RGN_HIST_SIZE],
+	std::vector<SSG_peakOrienRgnInfo>& objOps,
+#if OUTPUT_DEBUG
+	std::vector<std::vector< SVzNL3DPosition>>& bagPositionCloudPts,
+#endif
+	int* errCode);
+
 SG_APISHARED_EXPORT void sg_sideBagPosition(
 	SVzNL3DLaserLine* laser3DPoints,
 	int lineNum,
--- a/sourceCode/SG_baseAlgo_Export.h
+++ b/sourceCode/SG_baseAlgo_Export.h
@ -75,6 +75,14 @@ SG_APISHARED_EXPORT void sg_getLineLocalPeaks(
    std::vector< SSG_basicFeature1D>& localMax,
    std::vector< SSG_basicFeature1D>& localMin);

+SG_APISHARED_EXPORT void sg_getFlatLineLocalPeaks_vector(
+    std::vector<SVzNL3DPosition>& lineData,
+    int lineIdx,
+    const double scaleWin,
+    const double minPkHeighth,
+    const double holeR,
+    std::vector< SSG_basicFeature1D>& localMax);
+
 SG_APISHARED_EXPORT void sg_getLineDownJumps(
    SVzNL3DPosition* lineData,
    int dataSize,
@ -88,6 +96,13 @@ SG_APISHARED_EXPORT void sg_getFeatureGrowingTrees(
    std::vector<SSG_featureTree>& trees,
    SSG_treeGrowParam growParam);

+SG_APISHARED_EXPORT void sg_lineFeaturesGrowing(
+    int lineIdx,
+    bool isLastLine,
+    std::vector<SSG_basicFeature1D>& features,
+    std::vector<SSG_featureTree>& trees,
+    SSG_treeGrowParam growParam);
+
 //对ending进行生长
 SG_APISHARED_EXPORT void sg_getEndingGrowingTrees(
    std::vector<SSG_2DValueI>& lineEndings,
@ -212,6 +227,10 @@ SG_APISHARED_EXPORT void sg_getLineRigthAngleFeature(
 SG_APISHARED_EXPORT SVzNL3DRangeD sg_getScanDataROI(
    SVzNL3DLaserLine* laser3DPoints,
    int lineNum);
+//¼ÆËãÉ¨ÃèROI: vecotr¸ñÊ½
+SG_APISHARED_EXPORT SVzNL3DRangeD sg_getScanDataROI_vector(
+    std::vector< std::vector<SVzNL3DPosition>>& scanLines
+);

 //XY平面直线拟合
 SG_APISHARED_EXPORT void lineFitting(
@ -264,4 +283,18 @@ SG_APISHARED_EXPORT void eulerToRotationMatrixZYX(const SSG_EulerAngles& angles,
 SG_APISHARED_EXPORT void lineDataRT(
    SVzNL3DLaserLine* a_line,
    const double* camPoseR,
-    double groundH);
+    double groundH);
+SG_APISHARED_EXPORT void lineDataRT_vector(
+    std::vector< SVzNL3DPosition>& a_line,
+    const double* camPoseR, 
+    double groundH);
+SG_APISHARED_EXPORT void lineDataRT_RGBD(
+    SVzNLXYZRGBDLaserLine* a_line,
+    const double* camPoseR,
+    double groundH);
+
+SG_APISHARED_EXPORT void sg_pointClustering(
+    std::vector< SVzNL3DPosition>& pts,
+    double clusterDist,
+    std::vector<std::vector< SVzNL3DPosition>>& objClusters  //result
+);
--- a/sourceCode/SG_baseDataType.h
+++ b/sourceCode/SG_baseDataType.h
@ -2,6 +2,7 @@

 #include <VZNL_Types.h>
 #include <vector>
+#include <SG_errCode.h>

 #define PI 3.141592654

@ -55,6 +56,18 @@ typedef struct
 	double bottom;
 }SSG_ROIRectD;

+struct HSV {
+	double h; // 色相 (0-360)
+	double s; // 饱和度 (0-1)
+	double v; // 亮度 (0-1)
+};
+
+struct RGB {
+	int r; 
+	int g; 
+	int b; 
+};
+
 #define LINE_FEATURE_NUM			16
 #define LINE_FEATURE_UNDEF			0
 #define LINE_FEATURE_L_JUMP_H2L		1
@ -311,6 +324,15 @@ typedef struct
 	SSG_6DOF centerPos;
 }SSG_peakRgnInfo;

+typedef struct
+{
+	int pkRgnIdx;
+	SVzNLSizeD objSize;
+	SVzNL2DPoint pos2D;
+	SSG_6DOF centerPos;
+	int orienFlag;  //0-未知，1 - 正面， 2-反面
+}SSG_peakOrienRgnInfo;
+
 typedef struct
 {
 	SVzNL3DPoint sPt;
--- a/sourceCode/SG_baseFunc.cpp
+++ b/sourceCode/SG_baseFunc.cpp
@ -64,6 +64,66 @@ SVzNL3DRangeD sg_getScanDataROI(
 	return roi;
 }

+SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector<SVzNL3DPosition>>& scanLines)
+{
+	SVzNL3DRangeD roi;
+	roi.xRange = { 0, -1 };
+	roi.yRange = { 0, -1 };
+	roi.zRange = { 0, -1 };
+
+	int lineNum = scanLines.size();
+	for (int line = 0; line < lineNum; line++)
+	{
+		int nPositionCnt = scanLines[line].size();
+		for (int i = 0; i < nPositionCnt; i++)
+		{
+			SVzNL3DPosition* pt3D = &scanLines[line][i];
+			if (pt3D->pt3D.z < 1e-4)
+				continue;
+
+			if (roi.xRange.max < roi.xRange.min)
+			{
+				roi.xRange.min = pt3D->pt3D.x;
+				roi.xRange.max = pt3D->pt3D.x;
+			}
+			else
+			{
+				if (roi.xRange.min > pt3D->pt3D.x)
+					roi.xRange.min = pt3D->pt3D.x;
+				if (roi.xRange.max < pt3D->pt3D.x)
+					roi.xRange.max = pt3D->pt3D.x;
+			}
+			//y
+			if (roi.yRange.max < roi.yRange.min)
+			{
+				roi.yRange.min = pt3D->pt3D.y;
+				roi.yRange.max = pt3D->pt3D.y;
+			}
+			else
+			{
+				if (roi.yRange.min > pt3D->pt3D.y)
+					roi.yRange.min = pt3D->pt3D.y;
+				if (roi.yRange.max < pt3D->pt3D.y)
+					roi.yRange.max = pt3D->pt3D.y;
+			}
+			//z
+			if (roi.zRange.max < roi.zRange.min)
+			{
+				roi.zRange.min = pt3D->pt3D.z;
+				roi.zRange.max = pt3D->pt3D.z;
+			}
+			else
+			{
+				if (roi.zRange.min > pt3D->pt3D.z)
+					roi.zRange.min = pt3D->pt3D.z;
+				if (roi.zRange.max < pt3D->pt3D.z)
+					roi.zRange.max = pt3D->pt3D.z;
+			}
+		}
+	}
+	return roi;
+}
+
 void lineFitting(std::vector< SVzNL3DPoint>& inliers, double* _k, double* _b)
 {
 	//最小二乘拟合直线参数
@ -876,9 +936,10 @@ SSG_planeCalibPara sg_getPlaneCalibPara(
 			zHist[histPos] ++;
 		}
 	}
-	//以厘米为单位进行累加
 	std::vector<int> zSumHist;
 	zSumHist.resize(zHistSize);
+	bool isSame = true;
+	//以厘米为单位进行累加
 	for (int i = 0; i < zHistSize; i++)
 	{
 		int sumValue = 0;
@ -888,6 +949,17 @@ SSG_planeCalibPara sg_getPlaneCalibPara(
 				sumValue += zHist[j];
 		}
 		zSumHist[i] = sumValue;
+		if (i > 0)
+		{
+			if (sumValue != zSumHist[i - 1])
+				isSame = false;
+		}
+	}
+	if(true == isSame)
+	{
+		//不进行累加（如果累加，累加值相等）
+		for (int i = 0; i < zHistSize; i++)
+			zSumHist[i] = zHist[i];
 	}

 	//寻找极值
@ -936,6 +1008,8 @@ SSG_planeCalibPara sg_getPlaneCalibPara(
 				pkTop.push_back({pre_i, pre_data});
 				_state = 2;
 			}
+			else  if(i == (zHistSize-1))
+				pkTop.push_back({ i, curr_data });
 			break;
 		case 2: //下降
 			if (z_diff > 0)   //  上升
@ -945,6 +1019,12 @@ SSG_planeCalibPara sg_getPlaneCalibPara(
 				pkBtm.push_back({ pre_i, pre_data });
 				_state = 1;
 			}
+			else if (i == (zHistSize - 1))
+			{
+				int pkBtmIdx = pkBtm.size();
+				pkBtmBackIndexing[i] = pkBtmIdx;
+				pkBtm.push_back({ i, curr_data });
+			}
 			break;
 		default:
 			_state = 0;
@ -1377,4 +1457,42 @@ void lineDataRT(SVzNL3DLaserLine* a_line, const double* camPoseR, double groundH
 		a_line->p3DPosition[i].pt3D = a_pt;
 	}
 	return;
+}
+void lineDataRT_vector(std::vector< SVzNL3DPosition>& a_line, const double* camPoseR, double groundH)
+{
+	for (int i = 0; i < a_line.size(); i++)
+	{
+		SVzNL3DPoint a_pt = a_line[i].pt3D;
+		if (a_pt.z < 1e-4)
+			continue;
+		double x = a_pt.x * camPoseR[0] + a_pt.y * camPoseR[1] + a_pt.z * camPoseR[2];
+		double y = a_pt.x * camPoseR[3] + a_pt.y * camPoseR[4] + a_pt.z * camPoseR[5];
+		double z = a_pt.x * camPoseR[6] + a_pt.y * camPoseR[7] + a_pt.z * camPoseR[8];
+		if ((groundH > 0) && (z > groundH))  //去除地面
+			z = 0;
+		a_pt.x = x;
+		a_pt.y = y;
+		a_pt.z = z;
+		a_line[i].pt3D = a_pt;
+	}
+	return;
+}
+void lineDataRT_RGBD(SVzNLXYZRGBDLaserLine* a_line, const double* camPoseR, double groundH)
+{
+	for (int i = 0; i < a_line->nPointCnt; i++)
+	{
+		SVzNLPointXYZRGBA a_pt = a_line->p3DPoint[i];
+		if (a_pt.z < 1e-4)
+			continue;
+		double x = a_pt.x * camPoseR[0] + a_pt.y * camPoseR[1] + a_pt.z * camPoseR[2];
+		double y = a_pt.x * camPoseR[3] + a_pt.y * camPoseR[4] + a_pt.z * camPoseR[5];
+		double z = a_pt.x * camPoseR[6] + a_pt.y * camPoseR[7] + a_pt.z * camPoseR[8];
+		if ((groundH > 0) && (z > groundH))  //去除地面
+			z = 0;
+		a_pt.x = x;
+		a_pt.y = y;
+		a_pt.z = z;
+		a_line->p3DPoint[i] = a_pt;
+	}
+	return;
 }
--- a/sourceCode/SG_clustering.cpp
+++ b/sourceCode/SG_clustering.cpp
@ -0,0 +1,67 @@
+#include "SG_baseDataType.h"
+#include "SG_baseAlgo_Export.h"
+#include <vector>
+#include <corecrt_math_defines.h>
+#include <cmath>
+
+void _seedClustering(
+	std::vector< SVzNL3DPosition>& a_cluster,
+	std::vector< SVzNL3DPosition>& pts,
+	double clusterDist)
+{
+	int i = 0;
+	while (1)
+	{
+		if (i >= a_cluster.size())
+			break;
+
+		SVzNL3DPosition a_seed = a_cluster[i];
+		for (int i = 0, i_max = pts.size(); i < i_max; i++)
+		{
+			if (pts[i].nPointIdx < 0)
+				continue;
+			double dist = sqrt(pow(a_seed.pt3D.x - pts[i].pt3D.x, 2) + pow(a_seed.pt3D.y - pts[i].pt3D.y, 2));
+			if (dist < clusterDist)
+			{
+				a_cluster.push_back(pts[i]);
+				pts[i].nPointIdx = -1;
+			}
+		}
+		i++;
+	}
+	return;
+}
+
+void sg_pointClustering(
+	std::vector< SVzNL3DPosition>& pts, 
+	double clusterDist, 
+	std::vector<std::vector< SVzNL3DPosition>>& objClusters  //result
+)
+{
+	int ptSize = pts.size();
+	if (ptSize == 0)
+		return;
+	while(pts.size() > 0)
+	{
+		SVzNL3DPosition a_pt = pts[0];
+
+		//新建一个cluster
+		std::vector< SVzNL3DPosition> a_cluster;
+		a_cluster.push_back(a_pt);
+		pts[0].nPointIdx = -1; //防止重复被计算
+		_seedClustering(
+			a_cluster,
+			pts,
+			clusterDist);
+		objClusters.push_back(a_cluster); //保存一个聚类
+		//将pts中处理过的点去除
+		int m_max = pts.size();
+		for (int m = m_max - 1; m >= 0; m--)  //从后往前，这样删除不会影响循环
+		{
+			if(pts[m].nPointIdx < 0)
+				pts.erase(pts.begin() + m);
+		}
+	}
+	return;
+}
+;
--- a/sourceCode/SG_featureGrow.cpp
+++ b/sourceCode/SG_featureGrow.cpp
@ -407,6 +407,54 @@ void sg_getFeatureGrowingTrees(
 	}
 }

+void sg_lineFeaturesGrowing(
+	int lineIdx,
+	bool isLastLine,
+	std::vector<SSG_basicFeature1D>& features,
+	std::vector<SSG_featureTree>& trees,
+	SSG_treeGrowParam growParam)
+{
+	if (features.size() > 0)
+	{
+		for (int j = 0, j_max = features.size(); j < j_max; j++)
+		{
+			SSG_basicFeature1D& a_feature = features[j];
+			if (a_feature.jumpPos2D.x== 207)
+				int kkk = 1;
+			bool isMatched = _featureGrowing(a_feature, lineIdx, trees, growParam);
+			if (false == isMatched)
+			{
+				//新的生长树
+				SSG_featureTree a_newTree;
+				a_newTree.treeNodes.push_back(a_feature);
+				a_newTree.treeState = TREE_STATE_ALIVE;
+				a_newTree.treeType = a_feature.featureType;
+				a_newTree.sLineIdx = lineIdx;
+				a_newTree.eLineIdx = lineIdx;
+				trees.push_back(a_newTree);
+			}
+		}
+	}
+	//检查停止生长的树,加速。
+	//将生长节点为1的生长树移除
+	int m_max = trees.size();
+	for (int m = m_max - 1; m >= 0; m--)  //从后往前，这样删除不会影响循环
+	{
+		if (TREE_STATE_ALIVE == trees[m].treeState)
+		{
+			int line_diff = abs(lineIdx - trees[m].treeNodes.back().jumpPos2D.x);
+			if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) ||
+				(true == isLastLine))
+			{
+				trees[m].treeState = TREE_STATE_DEAD;
+				bool isValid = _invalidateVSlopeTrees(trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);
+				if (false == isValid)
+					trees.erase(trees.begin() + m);
+			}
+		}
+	}
+}
+
 void sg_getEndingGrowingTrees(
 	std::vector<SSG_2DValueI>& lineEndings,
 	SVzNL3DLaserLine* laser3DPoints,
--- a/sourceCode/SG_lineFeature.cpp
+++ b/sourceCode/SG_lineFeature.cpp
@ -149,7 +149,10 @@ if (i == 370)
 }

 //滤除离群点：z跳变门限方法
-void sg_lineDataRemoveOutlier_changeOriginData(SVzNL3DPosition* lineData, int dataSize, SSG_outlierFilterParam filterParam)
+void sg_lineDataRemoveOutlier_changeOriginData(
+	SVzNL3DPosition* lineData, 
+	int dataSize, 
+	SSG_outlierFilterParam filterParam)
 {
 	std::vector< SSG_RUN> continueRuns;
 	SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx
@ -1539,6 +1542,804 @@ void sg_getLineLocalPeaks(
 	return;
 }

+#if 0
+typedef struct
+{
+	double holeLen;
+	double holeCenterY;
+	double holeCenterX;
+	int holeSIdx;
+	int holeEIdx;
+}_HoleInfo;
+
+typedef struct
+{
+	double maxZ;
+	double minZ;
+	double pk_y;
+	double pk_x;
+	int pkIdx;
+}_Feature;
+//对调平后的直线寻找突起。采用分段法寻找。
+void sg_getFlatLineLocalPeaks_vector(
+	std::vector<SVzNL3DPosition>& lineData,
+	int lineIdx,
+	const double scaleWin,
+	const double minPkHeighth,
+	const double holeR,
+	std::vector< SSG_basicFeature1D>& localMax)
+{
+	int dataSize = lineData.size();
+	if (dataSize < 2)
+		return;
+
+	double maxZ = 0;
+	double subScale = scaleWin / 3.0;
+	std::vector<_Feature> subFeatures;
+	int maxIdx = -1;
+	int minIdx = -1;
+	bool startVld = false;
+	double startY = 0;
+	int preVldPt = 0;
+	int holePtNum = 0;
+	_HoleInfo a_hole = { 0.0, 0.0, 0.0, -1, -1 };
+	for (int i = 0; i < dataSize; i++)
+	{
+		lineData[i].nPointIdx = 0;
+		if ((lineIdx == 1219) && (i == 2722))
+			int kkk = 1;
+
+		if (lineData[i].pt3D.z < 1e-4)
+		{
+			if (true == startVld)
+				holePtNum++;
+		}
+		else
+		{
+			if (maxZ < lineData[i].pt3D.z)
+				maxZ = lineData[i].pt3D.z;
+
+			if (false == startVld)
+			{
+				startVld = true;
+				startY = lineData[i].pt3D.y;
+			}
+			//检查孔洞
+			if ((holePtNum > 0) && (preVldPt >= 0))
+			{
+				double holeLen = lineData[i].pt3D.y - lineData[preVldPt].pt3D.y;
+				if (holeLen > holeR / 3)
+				{
+					_HoleInfo currHole;
+					currHole.holeLen = holeLen;
+					currHole.holeCenterY = (lineData[i].pt3D.y + lineData[preVldPt].pt3D.y) / 2;
+					currHole.holeCenterX = (lineData[i].pt3D.x + lineData[preVldPt].pt3D.x) / 2;
+					currHole.holeSIdx = preVldPt;
+					currHole.holeEIdx = i;
+				
+					if (a_hole.holeSIdx >= 0) 
+					{
+						//检查孔洞是否可以合并，如果不能合并，则取大的孔洞
+						double hole_dist = currHole.holeCenterY - a_hole.holeCenterY;
+						if (hole_dist < holeR * 2)
+						{
+							a_hole.holeEIdx = currHole.holeEIdx;
+							a_hole.holeLen = lineData[a_hole.holeEIdx].pt3D.y - lineData[a_hole.holeSIdx].pt3D.y;
+							a_hole.holeCenterY = (lineData[a_hole.holeEIdx].pt3D.y + lineData[a_hole.holeSIdx].pt3D.y) / 2;
+						}
+						else if (a_hole.holeLen < currHole.holeLen)
+							a_hole = currHole;
+
+					}
+					else
+						a_hole = currHole;
+				}
+				//重新检查孔洞
+				holePtNum = 0;
+			}
+
+			double diffY = lineData[i].pt3D.y - startY;
+			if (diffY >= subScale)  //分段检查
+			{
+				if (a_hole.holeSIdx >= 0)
+				{
+					_Feature a_subFeature;
+					a_subFeature.minZ = -1;
+					a_subFeature.maxZ = -1;
+					a_subFeature.pk_y = a_hole.holeCenterY;
+					a_subFeature.pk_x = a_hole.holeCenterX;
+					a_subFeature.pkIdx = (a_hole.holeSIdx + a_hole.holeEIdx) / 2;
+					subFeatures.push_back(a_subFeature);
+				}
+				else if (maxIdx >= 0)
+				{
+					double depth = lineData[maxIdx].pt3D.z - lineData[minIdx].pt3D.z;
+					if (depth > minPkHeighth / 2)
+					{
+						_Feature a_subFeature;
+						a_subFeature.minZ = lineData[minIdx].pt3D.z;
+						a_subFeature.maxZ = lineData[maxIdx].pt3D.z;
+						a_subFeature.pk_x = lineData[maxIdx].pt3D.x;
+						a_subFeature.pk_y = lineData[maxIdx].pt3D.y;
+						a_subFeature.pkIdx = maxIdx;
+						subFeatures.push_back(a_subFeature);
+					}
+				}
+				//分段重新开始
+				maxIdx = -1;
+				minIdx = -1;
+				startY = lineData[i].pt3D.y;
+				a_hole = { 0.0, 0.0, 0.0, -1, -1 };
+			}
+
+			if (maxIdx < 0)
+				maxIdx = i;
+			else
+			{
+				if (lineData[maxIdx].pt3D.z < lineData[i].pt3D.z)
+					maxIdx = i;
+			}
+			if (minIdx < 0)
+				minIdx = i;
+			else
+			{
+				if (lineData[minIdx].pt3D.z > lineData[i].pt3D.z)
+					minIdx = i;
+			}
+
+			//处理最后一个数据
+			if (i == dataSize - 1)
+			{
+				if (a_hole.holeSIdx >= 0)
+				{
+					_Feature a_subFeature;
+					a_subFeature.minZ = -1;
+					a_subFeature.maxZ = -1;
+					a_subFeature.pk_x = a_hole.holeCenterX;
+					a_subFeature.pk_y = a_hole.holeCenterY;
+					a_subFeature.pkIdx = (a_hole.holeSIdx + a_hole.holeEIdx) / 2;
+					subFeatures.push_back(a_subFeature);
+				}
+				else if (maxIdx >= 0)
+				{
+					_Feature a_subFeature;
+					a_subFeature.minZ = lineData[minIdx].pt3D.z;
+					a_subFeature.maxZ = lineData[maxIdx].pt3D.z;
+					a_subFeature.pk_x = lineData[maxIdx].pt3D.x;
+					a_subFeature.pk_y = lineData[maxIdx].pt3D.y;
+					a_subFeature.pkIdx = maxIdx;
+					subFeatures.push_back(a_subFeature);
+				}
+			}
+			preVldPt = i;
+		}
+	}
+
+	for (int i = 0, i_max = subFeatures.size(); i < i_max; i++)
+	{
+		_Feature* currFeature = &subFeatures[i];
+		if (currFeature->pkIdx < 0)
+			continue;
+		if (currFeature->maxZ < 0)  // 孔洞
+		{
+			bool isCombined = false;
+			if (i < i_max - 1) //向后合并
+			{
+				_Feature* nxtFeature = &subFeatures[i + 1];
+				//检查孔洞是否可以合并，如果不能合并，则取大的孔洞
+				double hole_dist = nxtFeature->pk_y - currFeature->pk_y;
+				if (hole_dist < holeR*2)  //合并
+				{
+					if (nxtFeature->maxZ < 0)
+					{
+						nxtFeature->pkIdx = (currFeature->pkIdx + nxtFeature->pkIdx) / 2;
+						nxtFeature->pk_x = (currFeature->pk_x + nxtFeature->pk_x) / 2;
+						nxtFeature->pk_y = (currFeature->pk_y + nxtFeature->pk_y) / 2;
+					}
+					currFeature->pkIdx = -1; //合并标记
+					isCombined = true;
+				}
+			}
+			if (false == isCombined)
+			{
+				//生成特征
+				SSG_basicFeature1D a_feature;
+				a_feature.featureType = LINE_FEATURE_PEAK_TOP;
+				a_feature.jumpPos2D = { lineIdx, currFeature->pkIdx };
+				a_feature.jumpPos.x = currFeature->pk_x;
+				a_feature.jumpPos.y = currFeature->pk_y;
+				a_feature.jumpPos.z = maxZ;
+				localMax.push_back(a_feature);
+				lineData[currFeature->pkIdx].nPointIdx = LINE_FEATURE_PEAK_TOP; //nPointIdx被转义使用
+				lineData[currFeature->pkIdx].pt3D = a_feature.jumpPos;
+			}
+		}
+		else
+		{
+			bool isBigger_1 = true; //比前一个大
+			bool isBigger_2 = true; //比后一个大
+			double minZ = currFeature->minZ;
+
+			if (i > 0)
+			{
+				_Feature* preFeature = &subFeatures[i - 1];
+				if (preFeature->maxZ < 0)
+				{
+					if(preFeature->pkIdx >= 0)
+						isBigger_1 = false;
+				}
+				else
+				{
+					if (currFeature->minZ > preFeature->minZ)
+						minZ = preFeature->minZ;
+					if (currFeature->maxZ < preFeature->maxZ)
+						isBigger_1 = false;
+				}
+			}
+			if (i < i_max - 1)
+			{
+				_Feature* nxtFeature = &subFeatures[i + 1];
+				if (nxtFeature->maxZ < 0)
+				{
+					double hole_dist = nxtFeature->pk_y - currFeature->pk_y;
+					if (hole_dist >= holeR * 2)
+						isBigger_2 = false;
+					else
+						nxtFeature->pkIdx = -1; //后面的孔被合并
+				}
+				else
+				{
+					if (currFeature->minZ > nxtFeature->minZ)
+						minZ = nxtFeature->minZ;
+					if (currFeature->maxZ < nxtFeature->maxZ)
+						isBigger_2 = false;
+				}
+			}
+			if ((true == isBigger_1) && (true == isBigger_2))
+			{
+				double z_diff = currFeature->maxZ - minZ;
+				if (z_diff > minPkHeighth)  //合格的突起
+				{
+					SSG_basicFeature1D a_feature;
+					a_feature.featureType = LINE_FEATURE_PEAK_TOP;
+					a_feature.jumpPos2D = { lineIdx, currFeature->pkIdx };
+					a_feature.jumpPos = lineData[currFeature->pkIdx].pt3D;
+					localMax.push_back(a_feature);
+					lineData[currFeature->pkIdx].nPointIdx = LINE_FEATURE_PEAK_TOP; //nPointIdx被转义使用
+				}
+			}
+		}
+	}
+	return;
+}
+#else
+//对调平后的直线寻找突起。
+//根据斜率寻找：寻找跳变、斜坡、Gap，进行配对
+typedef struct
+{
+	int type;
+	int ptIdxS;
+	int ptIdxE;
+	int sIdx;
+	int eIdx;
+	SVzNL3DPoint start;
+	SVzNL3DPoint end;
+	double delta_y;
+	double delta_z;
+	double k;
+}_FeatureInfo;
+
+int _findSegMaxZPt(
+	std::vector<SVzNL3DPosition>& lineData, 
+	int searchStart, 
+	bool searchToHead, 
+	double searchWin, 
+	double* meanZ,
+	bool* foundJump)
+{
+	int dataSize = lineData.size();
+	double start_y = lineData[searchStart].pt3D.y;
+	double start_z = lineData[searchStart].pt3D.z;
+	if (start_z < 1e-4)
+		return -1; 
+
+	double max_z = start_z;
+	int max_idx = searchStart;
+	double sum_z = max_z;
+	int sum_num = 1;
+	int step = searchToHead == true ? -1 : 1;
+	int idx = searchStart;
+	while (1)
+	{
+		idx += step;
+		if ((idx <= 0) || (idx >= dataSize))
+			break;
+
+		if (lineData[idx].nPointIdx == LINE_FEATURE_NUM)
+		{
+			*foundJump = true;
+			break;
+		}
+		double pt_y = lineData[idx].pt3D.y;
+		double pt_z = lineData[idx].pt3D.z;
+		if (pt_z > 1e-4)
+		{
+			double dist = abs(pt_y - start_y);
+			if (dist > searchWin)
+			{
+				*meanZ = sum_z / sum_num;
+				return max_idx;
+			}
+			else
+			{
+				sum_z += pt_z;
+				sum_num++;
+				if (max_z < pt_z)
+				{
+					max_z = pt_z;
+					max_idx = idx;
+				}
+			}
+
+		}
+	}
+	return -1;
+}
+
+void sg_getFlatLineLocalPeaks_vector(
+	std::vector<SVzNL3DPosition>& lineData,
+	int lineIdx,
+	const double scaleWin,
+	const double minPkHeighth,
+	const double holeR,
+	std::vector< SSG_basicFeature1D>& localMax)
+{
+	int dataSize = lineData.size();
+	if (dataSize < 2)
+		return;
+
+	//加速，进行抽样
+	//抽样的点中去除空点
+	double slopeZDeltaTH = 0.2; //slope的z变化门限，大于此门限为有限slope
+	int dwnSample = 1;
+	int sampleNum = dataSize / dwnSample;
+	std::vector<SVzNL3DPosition> sampleData;
+	for (int i = 0; i < sampleNum; i++)
+	{
+		SVzNL3DPosition a_sample = lineData[i * dwnSample];
+		a_sample.nPointIdx = i * dwnSample;
+		if(a_sample.pt3D.z > 1e-4)
+			sampleData.push_back(a_sample);
+	}
+
+	sampleNum = sampleData.size();
+	std::vector< _FeatureInfo> allSubFeatures;
+	allSubFeatures.resize(sampleNum);
+	for (int i = 0; i < sampleNum; i++)  //初始化
+		allSubFeatures[i].type = LINE_FEATURE_UNDEF;
+
+	int segEnd = 0;
+	for (int i = 0; i < sampleNum-1; i++)
+	{
+		SVzNL3DPosition* samplePtr = &sampleData[i];
+		if ((lineIdx == 0) && (i == 2290))
+			int kkk = 1;
+		//检查相邻两点是否为跳变或gap
+		SVzNL3DPosition* nxtSamplePtr = &sampleData[i+1];
+		//double d_delta = sqrt(pow(nxtSamplePtr->pt3D.y - samplePtr->pt3D.y, 2) + pow(nxtSamplePtr->pt3D.z - samplePtr->pt3D.z, 2));
+		double y_delta = abs(nxtSamplePtr->pt3D.y - samplePtr->pt3D.y);
+		double z_delta = nxtSamplePtr->pt3D.z - samplePtr->pt3D.z;
+		if ((y_delta > holeR / 2) || (abs(z_delta) > minPkHeighth))
+		{
+			_FeatureInfo a_subFeature;
+			a_subFeature.delta_y = y_delta;
+			a_subFeature.delta_z = z_delta;
+			a_subFeature.k = z_delta / y_delta;
+			a_subFeature.ptIdxS = i;
+			a_subFeature.ptIdxE = i+1;
+			a_subFeature.sIdx = sampleData[i].nPointIdx;
+			a_subFeature.eIdx = sampleData[i + 1].nPointIdx;
+			a_subFeature.start = samplePtr->pt3D;
+			a_subFeature.end = nxtSamplePtr->pt3D;
+			if(z_delta > 0)
+				a_subFeature.type = LINE_FEATURE_L_JUMP_L2H;
+			else
+				a_subFeature.type = LINE_FEATURE_L_JUMP_H2L;
+			allSubFeatures[i] = a_subFeature;
+			segEnd = i + 1;
+		}
+		else
+		{
+			//寻找尺度端点
+			for (int j = segEnd; j < sampleNum; j++)
+			{
+				SVzNL3DPosition* postSamplePtr = &sampleData[j];
+				double deltaY = abs(postSamplePtr->pt3D.y - samplePtr->pt3D.y);
+				if (deltaY >= scaleWin) 
+				{
+					double deltaZ = postSamplePtr->pt3D.z - samplePtr->pt3D.z;
+					if (abs(deltaZ) > slopeZDeltaTH)
+						int kkk = 1;
+					_FeatureInfo a_subFeature;
+					a_subFeature.delta_y = deltaY;
+					a_subFeature.delta_z = deltaZ;
+					a_subFeature.k = deltaZ / deltaY;
+					a_subFeature.ptIdxS = i;
+					a_subFeature.ptIdxE = j;
+					a_subFeature.sIdx = sampleData[i].nPointIdx;
+					a_subFeature.eIdx = sampleData[j].nPointIdx;
+					a_subFeature.start = samplePtr->pt3D;
+					a_subFeature.end = postSamplePtr->pt3D;
+					if ( (abs(a_subFeature.k) > 0.5) && (abs(deltaZ) > slopeZDeltaTH))
+					{
+						if (deltaZ > 0)
+							a_subFeature.type = LINE_FEATURE_L_SLOPE_L2H;
+						else
+							a_subFeature.type = LINE_FEATURE_L_SLOPE_H2L;
+					}
+					else
+						a_subFeature.type = LINE_FEATURE_UNDEF;
+					allSubFeatures[i] = a_subFeature;
+					segEnd = j;
+					break;
+				}
+			}
+		}
+	}
+
+
+	//处理JUMP, 将相邻的LINE_FEATURE_L_SLOPE_L2H无效
+	for (int i = 0; i < sampleNum; i++)
+	{
+		if (allSubFeatures[i].type == LINE_FEATURE_L_JUMP_L2H)
+		{
+			//将相邻的LINE_FEATURE_L_SLOPE_L2H无效
+			for (int j = i + 1; j < sampleNum; j++)
+			{
+				if (allSubFeatures[j].type == LINE_FEATURE_L_SLOPE_L2H)
+					allSubFeatures[j].type = LINE_FEATURE_UNDEF;
+				else
+					break;
+			}
+		}
+		else if (allSubFeatures[i].type == LINE_FEATURE_L_JUMP_H2L)
+		{
+			//将相邻的LINE_FEATURE_L_SLOPE_L2H无效
+			for (int j = i - 1; j >= 0; j--)
+			{
+				if (allSubFeatures[j].type == LINE_FEATURE_L_SLOPE_H2L)
+					allSubFeatures[j].type = LINE_FEATURE_UNDEF;
+				else
+					break;
+			}
+		}
+	}
+#if 0
+	std::vector< _FeatureInfo> subFeatures;
+	for (int i = 0; i < sampleNum; i++)
+	{
+		if (allSubFeatures[i].type != LINE_FEATURE_UNDEF)
+			subFeatures.push_back(allSubFeatures[i]);
+	}
+	_FeatureInfo a_nullFeature;
+	memset(&a_nullFeature, 0, sizeof(_FeatureInfo));
+	subFeatures.push_back(a_nullFeature);  //在结尾补上一个空，序列处理时会正确处理最后一个
+	int subfeatureSize = subFeatures.size();
+#endif
+	//将合格的subFeature挑选出来
+	int pre_type = LINE_FEATURE_UNDEF;
+	_FeatureInfo a_vldFeature;
+	std::vector< _FeatureInfo> vldFeatures;
+	for (int i = 0; i < sampleNum; i++)
+	{
+		if (pre_type == LINE_FEATURE_UNDEF)
+		{
+			if ( (allSubFeatures[i].type == LINE_FEATURE_L_JUMP_H2L) ||
+				(allSubFeatures[i].type == LINE_FEATURE_L_JUMP_L2H))
+			{
+				vldFeatures.push_back(allSubFeatures[i]);
+				pre_type = LINE_FEATURE_UNDEF;
+			}
+			else
+			{
+				a_vldFeature = allSubFeatures[i];
+				pre_type = allSubFeatures[i].type;
+			}
+		}
+		else
+		{
+			if (allSubFeatures[i].type == LINE_FEATURE_UNDEF)
+			{
+				vldFeatures.push_back(a_vldFeature);
+			}
+			else
+			{
+				if (((a_vldFeature.k < 0) && (allSubFeatures[i].k < 0)) ||
+					((a_vldFeature.k > 0) && (allSubFeatures[i].k > 0)))
+				{
+					if (abs(a_vldFeature.k) < abs(allSubFeatures[i].k))
+						a_vldFeature = allSubFeatures[i];
+				}
+				else
+				{
+					vldFeatures.push_back(a_vldFeature);
+					a_vldFeature = allSubFeatures[i];
+				}
+			}
+			pre_type = allSubFeatures[i].type;
+		}
+	}
+
+	//配对，生成特征
+	//优先对Jump进行配对
+	std::vector<SSG_basicFeature1D> featureBuffer;
+	featureBuffer.resize(vldFeatures.size());
+	for (int i = 0; i < vldFeatures.size(); i++) //初始化
+		featureBuffer[i].featureType = LINE_FEATURE_UNDEF;
+	for (int i = 0, i_max = vldFeatures.size(); i < i_max; i++)
+	{
+		if (vldFeatures[i].type == LINE_FEATURE_L_JUMP_H2L)
+		{
+			//判断z的幅度
+			if (abs(vldFeatures[i].delta_z) < minPkHeighth)  //没有起伏，可以匹配后面的JUMP
+			{
+				//生成特征
+				SSG_basicFeature1D a_feature;
+				a_feature.featureType = LINE_FEATURE_PEAK_TOP;
+				int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2;
+				a_feature.jumpPos2D = { lineIdx, centerIdx };
+				a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2;
+				a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2;
+				a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2;
+				int start = vldFeatures[i].sIdx;
+				int end = vldFeatures[i].eIdx;
+				//尝试匹配后面的JUMP
+				if (i < i_max - 1)
+				{
+					if ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_H2L) ||
+						(vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_L2H))
+					{
+						//判断距离
+						double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y);
+						if (dist < holeR) //合并
+						{
+							centerIdx = (vldFeatures[i].sIdx + vldFeatures[i+1].eIdx) / 2;
+							a_feature.jumpPos2D = { lineIdx, centerIdx };
+							a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i+1].end.x) / 2;
+							a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i+1].end.y) / 2;
+							a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i+1].end.z) / 2;
+							vldFeatures[i + 1].type = LINE_FEATURE_UNDEF;
+							end = vldFeatures[i+1].eIdx;
+						}
+					}
+				}
+				featureBuffer[i] = a_feature;
+				lineData[start].nPointIdx = LINE_FEATURE_NUM;  //起点和终点作标识。使用LINE_FEATURE_NUM作标识码
+				lineData[end].nPointIdx = LINE_FEATURE_NUM;
+			}
+			else //有明显下降，只能匹配后面没有起伏的JUMP
+			{
+				//生成特征
+				SSG_basicFeature1D a_feature;
+				a_feature.featureType = LINE_FEATURE_PEAK_TOP;
+				int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2;
+				a_feature.jumpPos2D = { lineIdx, centerIdx };
+				a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2;
+				a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2;
+				a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2;
+				int start = vldFeatures[i].sIdx;
+				int end = vldFeatures[i].eIdx;
+				if (i < i_max - 1)
+				{
+					if ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_H2L) ||
+						(vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_L2H))
+					{
+						if (abs(vldFeatures[i + 1].delta_z) < minPkHeighth)  //只匹配没有起伏的
+						{
+							//判断距离
+							double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y);
+							if (dist < holeR) //合并
+							{
+								centerIdx = (vldFeatures[i].sIdx + vldFeatures[i + 1].eIdx) / 2;
+								a_feature.jumpPos2D = { lineIdx, centerIdx };
+								a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i + 1].end.x) / 2;
+								a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i + 1].end.y) / 2;
+								a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i + 1].end.z) / 2;
+								vldFeatures[i + 1].type = LINE_FEATURE_UNDEF;
+								end = vldFeatures[i + 1].eIdx;
+							}
+						}
+					}
+				}
+				featureBuffer[i] = a_feature;
+				lineData[start].nPointIdx = LINE_FEATURE_NUM;  //起点和终点作标识。使用LINE_FEATURE_NUM作标识码
+				lineData[end].nPointIdx = LINE_FEATURE_NUM;
+			}
+		}
+		else if (vldFeatures[i].type == LINE_FEATURE_L_JUMP_L2H)
+		{
+			//判断z的幅度
+			if (abs(vldFeatures[i].delta_z) < minPkHeighth) //没有起伏，可以匹配后面的JUMP
+			{
+				//直接匹配后面的JUMP
+				//生成特征
+				SSG_basicFeature1D a_feature;
+				a_feature.featureType = LINE_FEATURE_PEAK_TOP;
+				int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2;
+				a_feature.jumpPos2D = { lineIdx, centerIdx };
+				a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2;
+				a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2;
+				a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2;
+				int start = vldFeatures[i].sIdx;
+				int end = vldFeatures[i].eIdx;
+				//尝试匹配后面的JUMP
+				if (i < i_max - 1)
+				{
+					if ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_H2L) ||
+						(vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_L2H))
+					{
+						//判断距离
+						double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y);
+						if (dist < holeR) //合并
+						{
+							centerIdx = (vldFeatures[i].sIdx + vldFeatures[i + 1].eIdx) / 2;
+							a_feature.jumpPos2D = { lineIdx, centerIdx };
+							a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i + 1].end.x) / 2;
+							a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i + 1].end.y) / 2;
+							a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i + 1].end.z) / 2;
+							vldFeatures[i + 1].type = LINE_FEATURE_UNDEF;
+							end = vldFeatures[i + 1].eIdx;
+						}
+					}
+				}
+				featureBuffer[i] = a_feature;
+				lineData[start].nPointIdx = LINE_FEATURE_NUM;  //起点和终点作标识。使用LINE_FEATURE_NUM作标识码
+				lineData[end].nPointIdx = LINE_FEATURE_NUM;
+			}
+			else //有起伏的，可以匹配后面的下降JUMP和没有起伏的JUMP和SLOPE
+			{
+				//生成特征
+				SSG_basicFeature1D a_feature;
+				a_feature.featureType = LINE_FEATURE_PEAK_TOP;
+				int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2;
+				a_feature.jumpPos2D = { lineIdx, centerIdx };
+				a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2;
+				a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2;
+				a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2;
+				int start = vldFeatures[i].sIdx;
+				int end = vldFeatures[i].eIdx;
+				if (i < i_max - 1)
+				{
+					if ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_H2L) ||
+						((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_L2H) && (abs(vldFeatures[i+1].delta_z) < minPkHeighth)) ||
+						(vldFeatures[i + 1].type == LINE_FEATURE_L_SLOPE_H2L))
+					{
+						//判断距离
+						double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y);
+						if (dist < holeR) //合并
+						{
+							centerIdx = (vldFeatures[i].sIdx + vldFeatures[i + 1].eIdx) / 2;
+							a_feature.jumpPos2D = { lineIdx, centerIdx };
+							a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i + 1].end.x) / 2;
+							a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i + 1].end.y) / 2;
+							a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i + 1].end.z) / 2;
+							vldFeatures[i + 1].type = LINE_FEATURE_UNDEF;
+							end = vldFeatures[i + 1].eIdx;
+						}
+						
+					}
+				}
+				featureBuffer[i] = a_feature;
+				lineData[start].nPointIdx = LINE_FEATURE_NUM;  //起点和终点作标识。使用LINE_FEATURE_NUM作标识码
+				lineData[end].nPointIdx = LINE_FEATURE_NUM;
+			}
+		}
+	}
+	//对SLOPE进行配对
+	for (int i = 0, i_max = vldFeatures.size(); i < i_max; i++)
+	{
+		if (vldFeatures[i].type == LINE_FEATURE_L_SLOPE_L2H)
+		{
+			if (i < i_max - 1)
+			{
+				double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y);
+				if ((dist < holeR) && (vldFeatures[i + 1].type == LINE_FEATURE_L_SLOPE_H2L))
+				{
+					//生成特征
+					SSG_basicFeature1D a_feature;
+					a_feature.featureType = LINE_FEATURE_PEAK_TOP;
+					int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i + 1].eIdx) / 2;
+					a_feature.jumpPos2D = { lineIdx, centerIdx };
+					a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i + 1].end.x) / 2;
+					a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i + 1].end.y) / 2;
+					a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i + 1].end.z) / 2;
+					vldFeatures[i + 1].type = LINE_FEATURE_UNDEF;
+					featureBuffer[i] = a_feature;
+				}
+				else
+				{
+					SSG_basicFeature1D a_feature;
+					a_feature.featureType = LINE_FEATURE_PEAK_TOP;
+#if 0
+					int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2;
+					a_feature.jumpPos2D = { lineIdx, centerIdx };
+					a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2;
+					a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2;
+					a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2;
+#endif
+					//在HoleR范围内寻找Peak点
+					double mean_z = -1;
+					bool foundJump = false;
+					int pkIdx = _findSegMaxZPt(lineData, vldFeatures[i].eIdx, false, holeR*1.5, &mean_z, &foundJump);
+					//反向寻找是否有Jump
+					double meanZ_inv = -1;
+					bool foundJump_inv = false;
+					int pkIdx_inv = _findSegMaxZPt(lineData, vldFeatures[i].sIdx, true, holeR * 1.5, &meanZ_inv, &foundJump_inv);
+					if ( (pkIdx >= 0)&&(false == foundJump_inv))
+					{
+						double mean_depth = mean_z - vldFeatures[i].start.z;
+						if (pkIdx_inv >= 0)
+							mean_depth = mean_z - meanZ_inv;
+						if (mean_depth >= slopeZDeltaTH/2)
+						{
+							a_feature.jumpPos2D = { lineIdx, pkIdx };
+							a_feature.jumpPos = lineData[pkIdx].pt3D;
+							featureBuffer[i] = a_feature;
+						}
+					}
+					
+				}
+			}
+		}
+		else if (vldFeatures[i].type == LINE_FEATURE_L_SLOPE_H2L)
+		{
+			//在HoleR范围内寻找Peak点
+			SSG_basicFeature1D a_feature;
+			a_feature.featureType = LINE_FEATURE_PEAK_TOP;
+#if 0
+			int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2;
+			a_feature.jumpPos2D = { lineIdx, centerIdx };
+			a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2;
+			a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2;
+			a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2;
+#endif
+			//在HoleR范围内寻找Peak点
+			double mean_z = -1;
+			bool foundJump = false;
+			int pkIdx = _findSegMaxZPt(lineData, vldFeatures[i].sIdx, true, holeR*1.5, &mean_z, &foundJump);
+			//反向寻找是否有Jump
+			double meanZ_inv = -1;
+			bool foundJump_inv = false;
+			int pkIdx_inv = _findSegMaxZPt(lineData, vldFeatures[i].eIdx, false, holeR * 1.5, &meanZ_inv, &foundJump_inv);
+			if ((pkIdx >= 0) && (false == foundJump_inv))
+			{
+				double mean_depth = mean_z - vldFeatures[i].start.z;
+				if (pkIdx_inv >= 0)
+					mean_depth = mean_z - meanZ_inv;
+				if (mean_depth >= slopeZDeltaTH/2)
+				{
+					a_feature.jumpPos2D = { lineIdx, pkIdx };
+					a_feature.jumpPos = lineData[pkIdx].pt3D;
+					featureBuffer[i] = a_feature;
+				}
+			}
+		}
+
+	}
+	for (int i = 0, i_max = vldFeatures.size(); i < i_max; i++)
+	{
+		if (featureBuffer[i].featureType != LINE_FEATURE_UNDEF)
+		{
+			localMax.push_back(featureBuffer[i]);
+			if (lineData[featureBuffer[i].jumpPos2D.y].pt3D.z < 1e-4)
+				lineData[featureBuffer[i].jumpPos2D.y].pt3D = featureBuffer[i].jumpPos;
+			lineData[featureBuffer[i].jumpPos2D.y].nPointIdx = LINE_FEATURE_PEAK_TOP; //nPointIdx被转义使用
+		}
+	}
+	return;
+}
+#endif
+
 /// <summary>
 /// 提取激光线上的下跳变点（Z值变大的跳变）
 /// 
--- a/sourceCode/WD_QRcode3Ddetection.cpp
+++ b/sourceCode/WD_QRcode3Ddetection.cpp
@ -0,0 +1,811 @@
+#include <vector>
+#include "SG_baseDataType.h"
+#include "SG_baseAlgo_Export.h"
+#include "WD_QRcode3Ddetection_Export.h"
+#include <opencv2/opencv.hpp>
+#include <limits>
+
+typedef struct
+{
+	int id;
+	double dist;
+	double angle;
+}_linkInfo;
+typedef struct
+{
+	int objId;
+	SVzNL3DPoint ptPos;
+	_linkInfo link[8]; //R, RT, T, LT, L, LB, B, RB
+}NodeOctLink;
+
+typedef struct
+{
+	SSG_ROIRectD roi;
+	int* qrCode;
+}DMCodeInfo;
+
+//将ply格式的数据恢复成扫描行的数据形式，从而方面按行进行处理
+void wd_getScanLines(
+	std::vector<SVzNL3DPoint>& scanData, 
+	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
+	int scan_rows)
+{
+	std::vector<SVzNL3DPosition> a_line;
+	for (int i = 0, i_max = scanData.size(); i < i_max; i++)
+	{
+		int idx = i % scan_rows;
+		if (0 == idx)
+		{
+			//新的一行
+			if (a_line.size() > 0)
+			{
+				scanLines.push_back(a_line);
+				a_line.clear();
+			}
+		}
+		SVzNL3DPoint a_pt = scanData[i];
+		SVzNL3DPosition a_idxPt;
+		a_idxPt.nPointIdx = 0;
+		a_idxPt.pt3D = a_pt;
+		a_line.push_back(a_idxPt);
+	}
+	if(a_line.size() > 0)
+		scanLines.push_back(a_line);
+	return;
+}
+
+SSG_planeCalibPara wd_getBaseCalibPara(
+	SVzNL3DLaserLine* laser3DPoints,
+	int lineNum)
+{
+	return sg_getPlaneCalibPara(laser3DPoints, lineNum);
+}
+
+void wd_lineDataR(std::vector< SVzNL3DPosition>& a_line,
+	const double* camPoseR,
+	double groundH)
+{
+	lineDataRT_vector(a_line, camPoseR, groundH);
+}
+
+SVzNL3DPoint _getObjCenter(SSG_featureTree& a_tree)
+{
+	SVzNL3DPoint centerPt = { 0, 0, 0 };
+	int nodeNum = 0;
+	for (int i = 0; i < a_tree.treeNodes.size(); i++)
+	{
+		centerPt.x += a_tree.treeNodes[i].jumpPos.x;
+		centerPt.y += a_tree.treeNodes[i].jumpPos.y;
+		centerPt.z += a_tree.treeNodes[i].jumpPos.z;
+		nodeNum++;
+	}
+	if (nodeNum > 0)
+	{
+		centerPt.x = centerPt.x / nodeNum;
+		centerPt.y = centerPt.y / nodeNum;
+		centerPt.z = centerPt.z / nodeNum;
+	}
+	return centerPt;
+}
+
+_linkInfo compareBestLinl(_linkInfo link_1, _linkInfo link_2, double bestDist)
+{
+	double dist_diff1 = abs(link_1.dist - bestDist);
+	double dist_diff2 = abs(link_2.dist - bestDist);
+	if (dist_diff1 < dist_diff2)
+		return link_1;
+	else
+		return link_2;
+}
+
+//建立Node间的上下左右的Link关系
+void _createNodeLinks(
+	std::vector< SVzNL3DPosition>& a_cluster,
+	std::vector< NodeOctLink>& clusterNodeLinks,
+	double row_space,
+	double col_space
+)
+{
+	int dirInvTbl[8] = { 4, 5, 6, 7, 0, 1, 2, 3 };
+	double diaDist = sqrt(pow(row_space, 2) + pow(col_space, 2));
+	int nodeSize = a_cluster.size();
+	for (int i = 0; i < nodeSize; i++)
+	{
+		SVzNL3DPosition& a_node = a_cluster[i];
+		NodeOctLink& a_nodeLink = clusterNodeLinks[i];
+		_linkInfo link[8];
+		for (int j = 0; j < 8; j++)
+		{
+			link[j].id = -1;
+			link[j].dist = 0.0;
+			link[j].angle = 0.0;
+		}
+		for (int j = i+1; j < nodeSize; j++)
+		{
+			SVzNL3DPosition& chk_node = a_cluster[j];
+			NodeOctLink& chk_nodeLink = clusterNodeLinks[j];
+			if (chk_node.nPointIdx < 0)
+				continue;
+
+			double dist = sqrt(pow(a_node.pt3D.x - chk_node.pt3D.x, 2) + pow(a_node.pt3D.y - chk_node.pt3D.y, 2));
+			if (dist < diaDist * 2) //粗过滤
+			{
+				double angle = atan2(a_node.pt3D.y - chk_node.pt3D.y, chk_node.pt3D.x - a_node.pt3D.x); //图像坐标系y方向与欧氏坐标系相么
+				angle = angle * 180.0 / PI;  //转为角度
+				if (angle < 0)
+					angle += 360;  //转成0-360度
+
+				_linkInfo a_link = { j, dist, angle };
+				if ((angle > 345) || (angle < 15))  //R:0
+				{
+					if (dist < col_space * 1.5)
+					{
+						if (link[0].id < 0)
+							link[0] = a_link;
+						else
+							link[0] = compareBestLinl(link[0], a_link, col_space);
+					}
+				}
+				else if ((angle > 30) && (angle < 60)) //RT:1
+				{
+					if (dist < diaDist * 1.5)
+					{
+						if (link[1].id < 0)
+							link[1] = a_link;
+						else
+							link[1] = compareBestLinl(link[1], a_link, diaDist);
+					}
+				}
+				else if ((angle > 75) && (angle < 105)) //T:2
+				{
+					if (dist < row_space * 1.5)
+					{
+						if (link[2].id < 0)
+							link[2] = a_link;
+						else
+							link[2] = compareBestLinl(link[2], a_link, row_space);
+					}
+				}
+				else if ((angle > 120) && (angle < 150)) //LT:3
+				{
+					if (dist < diaDist * 1.5)
+					{
+						if (link[3].id < 0)
+							link[3] = a_link;
+						else
+							link[3] = compareBestLinl(link[3], a_link, diaDist);
+					}
+				}
+				else if ((angle > 165) && (angle < 195)) //L:4
+				{
+					if (dist < col_space * 1.5)
+					{
+						if (link[4].id < 0)
+							link[4] = a_link;
+						else
+							link[4] = compareBestLinl(link[4], a_link, col_space);
+					}
+				}
+				else if ((angle > 210) && (angle < 240)) //LB:5
+				{
+					if (dist < diaDist * 1.5)
+					{
+						if (link[5].id < 0)
+							link[5] = a_link;
+						else
+							link[5] = compareBestLinl(link[5], a_link, diaDist);
+					}
+				}
+				else if ((angle > 255) && (angle < 285)) //B:6
+				{
+					if (dist < row_space * 1.5)
+					{
+						if (link[6].id < 0)
+							link[6] = a_link;
+						else
+							link[6] = compareBestLinl(link[6], a_link, row_space);
+					}
+				}
+				else if ((angle > 300) && (angle < 330)) //RB:7
+				{
+					if (dist < diaDist * 1.5)
+					{
+						if (link[7].id < 0)
+							link[7] = a_link;
+						else
+							link[7] = compareBestLinl(link[7], a_link, diaDist);
+					}
+				}
+			}
+		}
+
+		for (int j = 0; j < 8; j++)
+		{
+			if (link[j].id >= 0)
+			{
+				a_nodeLink.link[j] = link[j];
+				int linkId = link[j].id;
+				int dir_inv = dirInvTbl[j];
+				if (clusterNodeLinks[linkId].link[dir_inv].id < 0)
+				{
+					clusterNodeLinks[linkId].link[dir_inv].id = a_nodeLink.objId;
+					clusterNodeLinks[linkId].link[dir_inv].dist = link[j].dist;
+					clusterNodeLinks[linkId].link[dir_inv].angle = link[j].angle + 180;
+					if (clusterNodeLinks[linkId].link[dir_inv].angle >= 360)
+						clusterNodeLinks[linkId].link[dir_inv].angle = clusterNodeLinks[linkId].link[dir_inv].angle - 360;
+				}
+			}
+		}
+	}
+}
+
+//
+std::vector<int> _LTseedSearchRight(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
+{
+	NodeOctLink a_seed = startNode;
+	std::vector<int> result;
+	result.push_back(a_seed.objId);
+	while (1)
+	{
+		if (a_seed.link[0].id < 0)
+			break;
+		a_seed = clusterNodeLinks[a_seed.link[0].id];
+
+		if ((a_seed.link[1].id < 0) && (a_seed.link[2].id < 0) && (a_seed.link[3].id < 0))
+			result.push_back(a_seed.objId);
+		else
+			break;
+	}
+	return result;
+}
+std::vector<int> _LTseedSearchBottom(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
+{
+	NodeOctLink a_seed = startNode;
+	std::vector<int> result;
+	result.push_back(a_seed.objId);
+	while (1)
+	{
+		if (a_seed.link[6].id < 0)
+			break;
+		a_seed = clusterNodeLinks[a_seed.link[6].id];
+
+		if ((a_seed.link[3].id < 0) && (a_seed.link[4].id < 0) && (a_seed.link[5].id < 0))
+			result.push_back(a_seed.objId);
+		else
+			break;
+	}
+	return result;
+}
+int _foundDM_LTcorner(std::vector< NodeOctLink>& clusterNodeLinks, int rows, int cols)
+{
+	for (int i = 0, i_max = clusterNodeLinks.size(); i < i_max; i++)
+	{
+		//寻找种子向右向下
+		NodeOctLink a_seed = clusterNodeLinks[i];
+		if ((a_seed.link[0].id >= 0) && (a_seed.link[6].id >= 0) &&
+			(a_seed.link[1].id < 0)  && (a_seed.link[2].id < 0) &&
+			(a_seed.link[3].id < 0) && (a_seed.link[4].id < 0) &&
+			(a_seed.link[5].id < 0))  //合格的种子
+		{
+			std::vector<int> R_result = _LTseedSearchRight(a_seed, clusterNodeLinks);
+			std::vector<int> B_result = _LTseedSearchBottom(a_seed, clusterNodeLinks);
+			if (((int)R_result.size() == cols) && ((int)B_result.size() == rows))  //找到LT角点
+				return i;
+		}
+	}
+	return -1;
+}
+
+std::vector<int> _RTseedSearchLeft(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
+{
+	NodeOctLink a_seed = startNode;
+	std::vector<int> result;
+	result.push_back(a_seed.objId);
+	while (1)
+	{
+		if (a_seed.link[4].id < 0)
+			break;
+		a_seed = clusterNodeLinks[a_seed.link[4].id];
+
+		if ((a_seed.link[1].id < 0) && (a_seed.link[2].id < 0) && (a_seed.link[3].id < 0))
+			result.push_back(a_seed.objId);
+		else
+			break;
+	}
+	return result;
+}
+std::vector<int> _RTseedSearchBottom(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
+{
+	NodeOctLink a_seed = startNode;
+	std::vector<int> result;
+	result.push_back(a_seed.objId);
+	while (1)
+	{
+		if (a_seed.link[6].id < 0)
+			break;
+		a_seed = clusterNodeLinks[a_seed.link[6].id];
+
+		if ((a_seed.link[1].id < 0) && (a_seed.link[0].id < 0) && (a_seed.link[7].id < 0))
+			result.push_back(a_seed.objId);
+		else
+			break;
+	}
+	return result;
+}
+int _foundDM_RTcorner(std::vector< NodeOctLink>& clusterNodeLinks, int rows, int cols)
+{
+	for (int i = 0, i_max = clusterNodeLinks.size(); i < i_max; i++)
+	{
+		//寻找种子向右向下
+		NodeOctLink a_seed = clusterNodeLinks[i];
+		if ((a_seed.link[4].id >= 0) && (a_seed.link[6].id >= 0) &&
+			(a_seed.link[0].id < 0) && (a_seed.link[1].id < 0) &&
+			(a_seed.link[2].id < 0) && (a_seed.link[3].id < 0) &&
+			(a_seed.link[7].id < 0))  //合格的种子
+		{
+			std::vector<int> L_result = _RTseedSearchLeft(a_seed, clusterNodeLinks);
+			std::vector<int> B_result = _RTseedSearchBottom(a_seed, clusterNodeLinks);
+			if (((int)L_result.size() == cols) && ((int)B_result.size() == rows))  //找到LT角点
+				return i;
+		}
+	}
+	return -1;
+}
+
+std::vector<int> _LBseedSearchRight(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
+{
+	NodeOctLink a_seed = startNode;
+	std::vector<int> result;
+	result.push_back(a_seed.objId);
+	while (1)
+	{
+		if (a_seed.link[0].id < 0)
+			break;
+		a_seed = clusterNodeLinks[a_seed.link[0].id];
+
+		if ((a_seed.link[5].id < 0) && (a_seed.link[6].id < 0) && (a_seed.link[7].id < 0))
+			result.push_back(a_seed.objId);
+		else
+			break;
+	}
+	return result;
+}
+std::vector<int> _LBseedSearchTop(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
+{
+	NodeOctLink a_seed = startNode;
+	std::vector<int> result;
+	result.push_back(a_seed.objId);
+	while (1)
+	{
+		if (a_seed.link[2].id < 0)
+			break;
+		a_seed = clusterNodeLinks[a_seed.link[2].id];
+
+		if ((a_seed.link[3].id < 0) && (a_seed.link[4].id < 0) && (a_seed.link[5].id < 0))
+			result.push_back(a_seed.objId);
+		else
+			break;
+	}
+	return result;
+}
+int _foundDM_LBcorner(std::vector< NodeOctLink>& clusterNodeLinks, int rows, int cols)
+{
+	for (int i = 0, i_max = clusterNodeLinks.size(); i < i_max; i++)
+	{
+		//寻找种子向右向下
+		NodeOctLink a_seed = clusterNodeLinks[i];
+		if ((a_seed.link[0].id >= 0) && (a_seed.link[2].id >= 0) &&
+			(a_seed.link[3].id < 0) && (a_seed.link[4].id < 0) &&
+			(a_seed.link[5].id < 0) && (a_seed.link[6].id < 0) &&
+			(a_seed.link[7].id < 0))  //合格的种子
+		{
+			std::vector<int> L_result = _LBseedSearchRight(a_seed, clusterNodeLinks);
+			std::vector<int> B_result = _LBseedSearchTop(a_seed, clusterNodeLinks);
+			if (((int)L_result.size() == cols) && ((int)B_result.size() == rows))  //找到LT角点
+				return i;
+		}
+	}
+	return -1;
+}
+
+std::vector<int> _RBseedSearchLeft(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
+{
+	NodeOctLink a_seed = startNode;
+	std::vector<int> result;
+	result.push_back(a_seed.objId);
+	while (1)
+	{
+		if (a_seed.link[4].id < 0)
+			break;
+		a_seed = clusterNodeLinks[a_seed.link[4].id];
+
+		if ((a_seed.link[5].id < 0) && (a_seed.link[6].id < 0) && (a_seed.link[7].id < 0))
+			result.push_back(a_seed.objId);
+		else
+			break;
+	}
+	return result;
+}
+std::vector<int> _RBseedSearchTop(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
+{
+	NodeOctLink a_seed = startNode;
+	std::vector<int> result;
+	result.push_back(a_seed.objId);
+	while (1)
+	{
+		if (a_seed.link[2].id < 0)
+			break;
+		a_seed = clusterNodeLinks[a_seed.link[2].id];
+
+		if ((a_seed.link[1].id < 0) && (a_seed.link[0].id < 0) && (a_seed.link[7].id < 0))
+			result.push_back(a_seed.objId);
+		else
+			break;
+	}
+	return result;
+}
+int _foundDM_RBcorner(std::vector< NodeOctLink>& clusterNodeLinks, int rows, int cols)
+{
+	for (int i = 0, i_max = clusterNodeLinks.size(); i < i_max; i++)
+	{
+		//寻找种子向右向下
+		NodeOctLink a_seed = clusterNodeLinks[i];
+		if ((a_seed.ptPos.x > 19.2) && (a_seed.ptPos.y > -31))
+			int kkk = 1;
+		if ((a_seed.link[2].id >= 0) && (a_seed.link[4].id >= 0) &&
+			(a_seed.link[0].id < 0) && (a_seed.link[1].id < 0) &&
+			(a_seed.link[5].id < 0) && (a_seed.link[6].id < 0) &&
+			(a_seed.link[7].id < 0))  //合格的种子
+		{
+			std::vector<int> L_result = _RBseedSearchLeft(a_seed, clusterNodeLinks);
+			std::vector<int> B_result = _RBseedSearchTop(a_seed, clusterNodeLinks);
+			if (((int)L_result.size() == cols) && ((int)B_result.size() == rows))  //找到LT角点
+				return i;
+		}
+	}
+	return -1;
+}
+
+typedef struct
+{
+	int idx;
+	SVzNL2DPoint pt2D;
+}DMPos;
+
+void _getDMPosLinks(std::vector< DMPos>& posLink, std::vector< NodeOctLink>& clusterNodeLinks)
+{
+	int i = 0;
+	while (1)
+	{
+		if (i >= posLink.size())
+			break;
+		DMPos a_seed = posLink[i];
+		NodeOctLink& a_node = clusterNodeLinks[a_seed.idx];
+		if (a_node.objId >= 0)
+		{
+			if (a_node.link[0].id >= 0)
+			{
+				DMPos new_seed;
+				new_seed.idx = a_node.link[0].id;
+				new_seed.pt2D = { a_seed.pt2D.x + 1, a_seed.pt2D.y };
+				if (clusterNodeLinks[new_seed.idx].objId >= 0)
+				{
+					clusterNodeLinks[new_seed.idx].link[4].id = -1; //标记已经处理
+					posLink.push_back(new_seed);
+				}
+			}
+			if (a_node.link[1].id >= 0)
+			{
+				DMPos new_seed;
+				new_seed.idx = a_node.link[1].id;
+				new_seed.pt2D = { a_seed.pt2D.x + 1, a_seed.pt2D.y - 1 };
+				if (clusterNodeLinks[new_seed.idx].objId >= 0)
+				{
+					clusterNodeLinks[new_seed.idx].link[5].id = -1; //标记已经处理
+					posLink.push_back(new_seed);
+				}
+			}
+			if (a_node.link[2].id >= 0)
+			{
+				DMPos new_seed;
+				new_seed.idx = a_node.link[2].id;
+				new_seed.pt2D = { a_seed.pt2D.x, a_seed.pt2D.y - 1 };
+				if (clusterNodeLinks[new_seed.idx].objId >= 0)
+				{
+					clusterNodeLinks[new_seed.idx].link[6].id = -1; //标记已经处理
+					posLink.push_back(new_seed);
+				}
+			}
+			if (a_node.link[3].id >= 0)
+			{
+				DMPos new_seed;
+				new_seed.idx = a_node.link[3].id;
+				new_seed.pt2D = { a_seed.pt2D.x - 1, a_seed.pt2D.y - 1 };
+				clusterNodeLinks[new_seed.idx].link[7].id = -1; //标记已经处理
+				posLink.push_back(new_seed);
+			}
+			if (a_node.link[4].id >= 0)
+			{
+				DMPos new_seed;
+				new_seed.idx = a_node.link[4].id;
+				new_seed.pt2D = { a_seed.pt2D.x - 1, a_seed.pt2D.y };
+				if (clusterNodeLinks[new_seed.idx].objId >= 0)
+				{
+					clusterNodeLinks[new_seed.idx].link[0].id = -1; //标记已经处理
+					posLink.push_back(new_seed);
+				}
+			}
+			if (a_node.link[5].id >= 0)
+			{
+				DMPos new_seed;
+				new_seed.idx = a_node.link[5].id;
+				new_seed.pt2D = { a_seed.pt2D.x - 1, a_seed.pt2D.y + 1 };
+				if (clusterNodeLinks[new_seed.idx].objId >= 0)
+				{
+					clusterNodeLinks[new_seed.idx].link[1].id = -1; //标记已经处理
+					posLink.push_back(new_seed);
+				}
+			}
+			if (a_node.link[6].id >= 0)
+			{
+				DMPos new_seed;
+				new_seed.idx = a_node.link[6].id;
+				new_seed.pt2D = { a_seed.pt2D.x, a_seed.pt2D.y + 1 };
+				if (clusterNodeLinks[new_seed.idx].objId >= 0)
+				{
+					clusterNodeLinks[new_seed.idx].link[2].id = -1; //标记已经处理
+					posLink.push_back(new_seed);
+				}
+			}
+			if (a_node.link[7].id >= 0)
+			{
+				DMPos new_seed;
+				new_seed.idx = a_node.link[7].id;
+				new_seed.pt2D = { a_seed.pt2D.x + 1, a_seed.pt2D.y + 1 };
+				if (clusterNodeLinks[new_seed.idx].objId >= 0)
+				{
+					clusterNodeLinks[new_seed.idx].link[3].id = -1; //标记已经处理
+					posLink.push_back(new_seed);
+				}
+			}
+			clusterNodeLinks[a_seed.idx].objId = -1; //标记已经处理
+		}
+		i++;
+	}
+	return;
+}
+
+SSG_ROIRectD _getClusterROI(std::vector< SVzNL3DPosition>& a_cluster)
+{
+	SSG_ROIRectD roi = {0,0,0,0};
+	int nodeSize = a_cluster.size();
+	if (nodeSize == 0)
+		return roi;
+
+	roi = { a_cluster [0].pt3D.x, a_cluster[0].pt3D.x, a_cluster[0].pt3D.y, a_cluster[0].pt3D.y};
+	for (int i = 1; i < nodeSize; i++)
+	{
+		roi.left = roi.left > a_cluster[i].pt3D.x ? a_cluster[i].pt3D.x : roi.left;
+		roi.right = roi.right < a_cluster[i].pt3D.x ? a_cluster[i].pt3D.x : roi.right;
+		roi.top = roi.top > a_cluster[i].pt3D.y ? a_cluster[i].pt3D.y : roi.top;
+		roi.bottom = roi.bottom < a_cluster[i].pt3D.y ? a_cluster[i].pt3D.y : roi.bottom;
+	}
+	return roi;
+}
+
+void wd_QRcode3Ddetection(
+	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
+	const WD_QRcodeParam qrcode_param, 
+	std::vector< SVzNL3DPosition>& outObjPoints,
+	cv::Mat& dmCodeImg)
+{
+	std::vector<std::vector< SSG_basicFeature1D>> lineZMaxPts;
+	int lineNum = scanLines.size();
+
+	double scaleWin = qrcode_param.row_space / 5;
+	double minPkHeighth = qrcode_param.pointHoleDepth / 2;
+	double holeR = qrcode_param.pointHoleR;
+	for (int line = 0; line < lineNum; line++)
+	{
+		std::vector< SSG_basicFeature1D> a_lineMax;
+		sg_getFlatLineLocalPeaks_vector(
+			scanLines[line],
+			line,
+			scaleWin,
+			minPkHeighth,
+			holeR,
+			a_lineMax);
+		lineZMaxPts.push_back(a_lineMax);
+	}
+	//特征生长
+	SSG_treeGrowParam growParam;
+	growParam.yDeviation_max = holeR;//生长时相邻特征最大的Y偏差
+	growParam.zDeviation_max = holeR; //生长时相邻特征最大的Z偏差
+	growParam.maxLineSkipNum = 5;  //生长时相邻特征的最大线间隔， -1时使用maxDkipDistance
+	growParam.maxSkipDistance = -1; //若maxLineSkipNum为-1， 使用此参数.设为-1时，此参数无效
+	growParam.minLTypeTreeLen = holeR/2;  //生长树最少的节点数目。小于此数目的生长树被移除
+	growParam.minVTypeTreeLen = holeR/2;  //生长树最少的节点数目。小于此数目的生长树被移除
+
+	std::vector<SSG_featureTree> trees;
+	for (int line = 0; line < lineNum; line++)
+	{
+		std::vector< SSG_basicFeature1D>& a_lineMax = lineZMaxPts[line];
+		if (a_lineMax.size() == 0)
+			continue;
+
+		//对feature进行生长
+		bool isLastLine = false;
+		if (line == lineNum - 1)
+			isLastLine = true;
+		sg_lineFeaturesGrowing(
+			line,
+			isLastLine,
+			a_lineMax,
+			trees,
+			growParam);
+	}
+
+	//计算目标中心点，以此代表各个点
+	std::vector< SVzNL3DPosition> objPoints;
+	objPoints.resize(trees.size());
+	for (int i = 0, i_max = trees.size(); i < i_max; i++)
+	{
+		SVzNL3DPosition a_obj;
+		a_obj.nPointIdx = i;
+		a_obj.pt3D = _getObjCenter(trees[i]);
+		objPoints[i] = a_obj;
+	}
+	//将邻近的点合并
+	int objPtSize = objPoints.size();
+	for (int i = objPtSize - 1; i >= 0; i--)
+	{
+		SVzNL3DPosition a_pt = objPoints[i];
+		bool isCombined = false;
+		for (int j = i - 1; j >= 0; j--)
+		{
+			SVzNL3DPosition chk_pt = objPoints[j];
+			double dist = sqrt(pow(a_pt.pt3D.x - chk_pt.pt3D.x, 2) + pow(a_pt.pt3D.y - chk_pt.pt3D.y, 2));
+			if (dist < holeR * 2) //合并
+			{
+				objPoints[j].pt3D.x = (a_pt.pt3D.x + chk_pt.pt3D.x) / 2;
+				objPoints[j].pt3D.y = (a_pt.pt3D.y + chk_pt.pt3D.y) / 2;
+				objPoints[j].pt3D.z = (a_pt.pt3D.z + chk_pt.pt3D.z) / 2;
+				isCombined = true;
+				break;
+			}	
+		}
+		if (true == isCombined)
+			objPoints.erase(objPoints.begin() + i);
+	}
+	for (int i = 0, i_max = objPoints.size(); i < i_max; i++)
+		outObjPoints.push_back(objPoints[i]);
+
+	//聚类，分成一个个二维码区域
+	double clusterDist = (qrcode_param.row_space + qrcode_param.col_space) * 2;  //使用棋盘格距离
+	std::vector<std::vector< SVzNL3DPosition>> objClusters;
+	sg_pointClustering(
+		objPoints,
+		clusterDist,
+		objClusters  //result
+	);
+
+	std::vector<DMCodeInfo> qrCodeResult;
+	for (int i = 0, i_max = objClusters.size(); i < i_max; i++)
+	{
+		std::vector< SVzNL3DPosition>& a_cluster = objClusters[i];
+		int clustNodeSize = a_cluster.size();
+		//构建相邻关系
+		std::vector< NodeOctLink> clusterNodeLinks;
+		clusterNodeLinks.resize(clustNodeSize);
+		for (int m = 0; m < clustNodeSize; m++) //初始化
+		{
+			clusterNodeLinks[m].objId = m;
+			clusterNodeLinks[m].ptPos = a_cluster[m].pt3D;
+			for (int j = 0; j < 8; j++)
+			{
+				clusterNodeLinks[m].link[j].id = -1;
+				clusterNodeLinks[m].link[j].dist = 0.0;
+				clusterNodeLinks[m].link[j].angle = 0.0;
+			}
+		}
+		_createNodeLinks(a_cluster,	clusterNodeLinks, qrcode_param.row_space, qrcode_param.col_space);
+		//寻找DM二维码特征
+		int cornerIdx = 0;
+		int cornerNodeIdx = _foundDM_LTcorner(clusterNodeLinks, qrcode_param.rows, qrcode_param.cols);
+		if (cornerNodeIdx < 0)
+		{
+			cornerIdx = 1;
+			cornerNodeIdx = _foundDM_RTcorner(clusterNodeLinks, qrcode_param.rows, qrcode_param.cols);
+			if (cornerNodeIdx < 0)
+			{
+				cornerIdx = 2;
+				cornerNodeIdx = _foundDM_LBcorner(clusterNodeLinks, qrcode_param.rows, qrcode_param.cols);
+				if (cornerNodeIdx < 0)
+				{
+					cornerIdx = 3;
+					cornerNodeIdx = _foundDM_RBcorner(clusterNodeLinks, qrcode_param.rows, qrcode_param.cols);
+				}
+			}
+		}
+		if (cornerNodeIdx >= 0)
+		{
+			//提取二维码编码值
+			int rows = qrcode_param.rows;
+			int cols = qrcode_param.cols;
+
+			DMPos startPos;
+			startPos.idx = cornerNodeIdx;
+			if (cornerIdx == 0)  //LT
+				startPos.pt2D = { 0,0 };
+			else if (cornerIdx == 1)  //RT
+				startPos.pt2D = { 0,cols-1 };
+			else if(cornerIdx == 2) //LB
+				startPos.pt2D = { rows-1,0 };
+			else //RB
+				startPos.pt2D = { rows - 1, cols-1 };
+
+			std::vector< DMPos> posLinks;
+			posLinks.push_back(startPos);
+			_getDMPosLinks(posLinks, clusterNodeLinks);
+			
+			int* codeBuff = (int*)malloc(sizeof(int) * rows * cols);
+			memset(codeBuff, 0, sizeof(int) * qrcode_param.rows * qrcode_param.cols);
+			for (int m = 0, m_max = posLinks.size(); m < m_max; m++)
+			{
+				int col = posLinks[m].pt2D.x;
+				int row = posLinks[m].pt2D.y;
+				codeBuff[row * cols + col] = 1;
+			}
+			//
+			DMCodeInfo a_dmCode;
+			a_dmCode.roi = _getClusterROI(a_cluster);
+			a_dmCode.qrCode = codeBuff;
+			qrCodeResult.push_back(a_dmCode);
+		}
+	}
+	if (qrCodeResult.size() > 0)
+	{
+		SVzNL3DRangeD globalRoi = sg_getScanDataROI_vector(scanLines);
+		//恢复二维码编码图像
+		int dmNodeSize = 60;
+		int dmLen = dmNodeSize * qrcode_param.cols;
+		double scale = (qrCodeResult[0].roi.right - qrCodeResult[0].roi.left) / (double)dmLen;
+		int skip = dmNodeSize;
+		int img_cols = (int)((globalRoi.xRange.max - globalRoi.xRange.min) / scale);
+		img_cols = ((img_cols + 3) / 4) * 4;  //4对齐
+		int img_rows = (int)((globalRoi.yRange.max - globalRoi.yRange.min) / scale);
+		img_rows = ((img_rows + 3) / 4) * 4;  //4对齐
+		//加上空白边
+		img_cols += skip * 2;
+		img_rows += skip * 2;
+		dmCodeImg = cv::Mat::ones(img_rows, img_cols, CV_8UC1); 
+		for (int i = 0; i < qrCodeResult.size(); i++)
+		{
+			double x = qrCodeResult[i].roi.left;
+			double y = qrCodeResult[i].roi.top;
+			SVzNL2DPoint cornerPos;
+			cornerPos.x = (int)((x-globalRoi.xRange.min) / scale) + skip;
+			cornerPos.y = (int)((y-globalRoi.yRange.min) / scale) + skip;
+			for (int row = 0; row < qrcode_param.rows; row++)
+			{
+				for (int col = 0; col < qrcode_param.cols; col++)
+				{
+					SVzNL2DPoint nodePos;
+					nodePos.x = cornerPos.x + col * dmNodeSize;
+					nodePos.y = cornerPos.y + row * dmNodeSize;
+					if (qrCodeResult[i].qrCode[row * qrcode_param.cols + col] > 0)
+					{
+						for (int m = -dmNodeSize / 2; m < dmNodeSize / 2; m++)
+						{
+							for (int n = -dmNodeSize / 2; n < dmNodeSize / 2; n++)
+								dmCodeImg.at<uchar>(nodePos.y + m, nodePos.x + n) = 0;
+						}
+					}
+				}
+			}
+			free(qrCodeResult[i].qrCode);
+			qrCodeResult[i].qrCode = NULL;
+		}
+	}
+	return;
+}
--- a/sourceCode/WD_QRcode3Ddetection_Export.h
+++ b/sourceCode/WD_QRcode3Ddetection_Export.h
@ -0,0 +1,42 @@
+#pragma once
+
+#if defined(SG_API_LIBRARY)
+#  define SG_APISHARED_EXPORT __declspec(dllexport)
+#else
+#  define SG_APISHARED_EXPORT __declspec(dllimport)
+#endif
+
+#include "SG_baseDataType.h"
+#include <vector>
+#include <opencv2/opencv.hpp>
+
+typedef struct
+{
+	int rows;  //二维码行数
+	int cols; //二维码列数
+	double row_space; //行间距，单位mm
+	double col_space; //列间距，单位mm
+	double pointHoleDepth; //二维码点的深度，单位mm
+	double pointHoleR; //二维码点的半径，单位mm
+}WD_QRcodeParam;
+
+//将ply格式的数据恢复成扫描行的数据形式，从而方面按行进行处理
+SG_APISHARED_EXPORT void wd_getScanLines(
+	std::vector<SVzNL3DPoint>& scanData,
+	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
+	int scan_rows);
+
+SG_APISHARED_EXPORT SSG_planeCalibPara wd_getBaseCalibPara(
+	SVzNL3DLaserLine* laser3DPoints,
+	int lineNum);
+
+SG_APISHARED_EXPORT void wd_lineDataR(
+	std::vector< SVzNL3DPosition>& a_line,
+	const double* camPoseR,
+	double groundH);
+
+SG_APISHARED_EXPORT void wd_QRcode3Ddetection(
+	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
+	const WD_QRcodeParam qrcode_param,
+	std::vector< SVzNL3DPosition>& objPoints,
+	cv::Mat& dmCodeImg);
Author	SHA1	Message	Date
jerryzeng	497b98e2c3	添加了拆包算法中编织袋正反识别和朝向识别算法，以及相应的测试程序	2025-07-15 21:39:09 +08:00
jerryzeng	ae46895573	QRCode 3D detection algorithm initial check in	2025-07-15 21:37:09 +08:00
jerryzeng	71e8f12480	QRCode（DM code) algorithm and test projects initial check in	2025-07-15 21:35:15 +08:00