jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

添加了橡胶手套定位手套环的算法

Browse Source
This commit is contained in:
jerryzeng 2025-07-28 09:22:07 +08:00
parent 1aff330677
commit be5dbbccfb
13 changed files with 1702 additions and 397 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
SG_Algorithm.sln
View File

@ -76,6 +76,16 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "QRcode3Ddetection_test", "Q
{A7C72ED9-B81D-4C6A-BE38-D75199C824AA} = {A7C72ED9-B81D-4C6A-BE38-D75199C824AA}
EndProjectSection
EndProject
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "glovePositioning", "glovePositioning\glovePositioning.vcxproj", "{4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}"
ProjectSection(ProjectDependencies) = postProject
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
EndProjectSection
EndProject
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "glovePositioning_test", "glovePositioning_test\glovePositioning_test.vcxproj", "{7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}"
ProjectSection(ProjectDependencies) = postProject
{4060CE45-6235-4CA7-B64C-6E4E5CDDC34F} = {4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}
EndProjectSection
EndProject
Global
GlobalSection(SolutionConfigurationPlatforms) = preSolution
Debug|x64 = Debug|x64
@ -204,6 +214,22 @@ Global
{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
{4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Debug|x64.ActiveCfg = Debug|x64
{4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Debug|x64.Build.0 = Debug|x64
{4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Debug|x86.ActiveCfg = Debug|Win32
{4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Debug|x86.Build.0 = Debug|Win32
{4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Release|x64.ActiveCfg = Release|x64
{4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Release|x64.Build.0 = Release|x64
{4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Release|x86.ActiveCfg = Release|Win32
{4060CE45-6235-4CA7-B64C-6E4E5CDDC34F}.Release|x86.Build.0 = Release|Win32
{7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Debug|x64.ActiveCfg = Debug|x64
{7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Debug|x64.Build.0 = Debug|x64
{7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Debug|x86.ActiveCfg = Debug|Win32
{7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Debug|x86.Build.0 = Debug|Win32
{7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Release|x64.ActiveCfg = Release|x64
{7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Release|x64.Build.0 = Release|x64
{7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Release|x86.ActiveCfg = Release|Win32
{7C1EFAB7-06CC-4B7D-81BB-64C6FA5996FD}.Release|x86.Build.0 = Release|Win32
EndGlobalSection
GlobalSection(SolutionProperties) = preSolution
HideSolutionNode = FALSE

251
bagPositioning_test/bagPositioning_test.cpp
View File

@ -45,7 +45,7 @@ SVzNLPointXYZRGBA _ptRotate_RGBD(SVzNLPointXYZRGBA pt3D, double matrix3d[9])
#define DATA_VER_FROM_CUSTOM 2
#define VZ_LASER_LINE_PT_MAX_NUM 4096
SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName, int* scanLineNum, float* scanV,
int* dataCalib, int* scanMaxStamp, int* canClockUnit)
int* dataCalib, int* scanMaxStamp, int* canClockUnit, bool removeNullLines)
{
std::ifstream inputFile(fileName);
std::string linedata;
@ -64,175 +64,122 @@ SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName,
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 = (SVzNLXYZRGBDLaserLine*)malloc(sizeof(SVzNLXYZRGBDLaserLine) * (lineNum + 1));
memset(_scanLines, 0, sizeof(SVzNLXYZRGBDLaserLine) * (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 = (SVzNLXYZRGBDLaserLine*)malloc(sizeof(SVzNLXYZRGBDLaserLine) * (lineNum + 1));
memset(_scanLines, 0, sizeof(SVzNLXYZRGBDLaserLine) * (lineNum + 1));
if (scanLineNum)
*scanLineNum = lines;
}
if (_scanLines == NULL)
return NULL;
int ptNum = 0;
int lineIdx = -1;
int lineIdx = 0;
int ptIdx = 0;
SVzNLPointXYZRGBA* p3DPoint = NULL;
if (dataFileVer == DATA_VER_NEW)
int ptNum = 0;
std::vector< SVzNLPointXYZRGBA> a_line;
int vldPtNum = 0;
unsigned int timeStamp = 0;
while (getline(inputFile, linedata))
{
while (getline(inputFile, linedata))
if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10))
{
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 = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * ptNum);
memset(p3DPoint, 0, sizeof(SVzNLPointXYZRGBA) * ptNum);
}
else
p3DPoint = NULL;
_scanLines[lineIdx].nPointCnt = 0;
_scanLines[lineIdx].nTimeStamp = timeStamp;
_scanLines[lineIdx].p3DPoint = p3DPoint;
}
else if (0 == strncmp("{", linedata.c_str(), 1))
{
float X, Y, Z;
int imageY = 0;
float leftX, leftY;
float rightX, rightY;
float r, g, b;
sscanf_s(linedata.c_str(), "{%f,%f,%f,%f,%f,%f }-{%f,%f}-{%f,%f}", &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY);
int id = _scanLines[lineIdx].nPointCnt;
if (id < ptNum)
{
if (lineIdx == 537)
int kkk = 1;
p3DPoint[id].x = X;
p3DPoint[id].y = Y;
p3DPoint[id].z = Z;
int nr = (int)(r * 255);
int ng = (int)(g * 255);
int nb = (int)(b * 255);
nb <<= 8;
nb += ng;
nb <<= 8;
nb += nr;
p3DPoint[id].nRGB = nb;
_scanLines[lineIdx].nPointCnt = id + 1;
}
}
double lineV = 0;
sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV);
if (scanV)
*scanV = (float)lineV;
}
}
else if (dataFileVer == DATA_VER_OLD)
{
while (getline(inputFile, linedata))
else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12))
{
if (0 == strncmp("DataElements_", linedata.c_str(), 13))
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 curr_timeStamp;
sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &curr_timeStamp, &ptNum);
if (firstIndex < 0)
firstIndex = lineIndex;
lineIndex = lineIndex - firstIndex;
if ((lineIndex < 0) || (lineIndex >= lines))
break;
int recvPtNum = (int)a_line.size();
if ( (recvPtNum == ptNum) && ((vldPtNum > 0) || (false == removeNullLines)))
{
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 = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * VZ_LASER_LINE_PT_MAX_NUM);
memset(p3DPoint, 0, sizeof(SVzNLPointXYZRGBA) * VZ_LASER_LINE_PT_MAX_NUM);
_scanLines[lineIdx].nPointCnt = 0;
SVzNLPointXYZRGBA* p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * ptNum);
_scanLines[lineIdx].nPointCnt = ptNum;
_scanLines[lineIdx].nTimeStamp = timeStamp;
_scanLines[lineIdx].p3DPoint = p3DPoint;
for (int m = 0; m < ptNum; m++)
p3DPoint[m] = a_line[m];
lineIdx++;
}
else if (0 == strncmp("(", linedata.c_str(), 1))
//new Line
timeStamp = curr_timeStamp;
vldPtNum = 0;
a_line.clear();
}
else if (0 == strncmp("{", linedata.c_str(), 1))
{
float X, Y, Z;
int imageY = 0;
float leftX, leftY;
float rightX, rightY;
float r, g, b;
sscanf_s(linedata.c_str(), "{%f,%f,%f,%f,%f,%f }-{%f,%f}-{%f,%f}", &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY);
{
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].nPointCnt;
if (id < VZ_LASER_LINE_PT_MAX_NUM)
{
p3DPoint[id].x = X;
p3DPoint[id].y = Y;
p3DPoint[id].z = Z;
p3DPoint[id].nRGB = 0;
_scanLines[lineIdx].nPointCnt = id + 1;
}
if (lineIdx == 537)
int kkk = 1;
SVzNLPointXYZRGBA a_pt;
a_pt.x = X;
a_pt.y = Y;
a_pt.z = Z;
int nr = (int)(r * 255);
int ng = (int)(g * 255);
int nb = (int)(b * 255);
nb <<= 8;
nb += ng;
nb <<= 8;
nb += nr;
a_pt.nRGB = nb;
if (a_pt.z > 1e-4)
vldPtNum++;
a_line.push_back(a_pt);
}
}
}
//last line
int recvPtNum = (int)a_line.size();
if ((recvPtNum == ptNum) && ((vldPtNum > 0) || (false == removeNullLines)))
{
SVzNLPointXYZRGBA* p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * ptNum);
_scanLines[lineIdx].nPointCnt = ptNum;
_scanLines[lineIdx].nTimeStamp = timeStamp;
_scanLines[lineIdx].p3DPoint = p3DPoint;
for (int m = 0; m < ptNum; m++)
p3DPoint[m] = a_line[m];
lineIdx++;
}
if (scanLineNum)
*scanLineNum = lineIdx;
inputFile.close();
return _scanLines;
}
@ -2578,7 +2525,7 @@ int main()
SVzNLRange fileIdx[TEST_GROUP] = {
{0,176},{1,200},{1,166},{122,141},{1,65},
{1,29},{108,135},{0,200}, {1,200}, {1,12},
{2,4}, {1,5}, {1,1}, {1,1},
{2,4}, {1,5}, {1,1}, {1,3},
{1,21},{1,28},
{3,3}, {1,51}, {4,83}, {1,74}, {1,61}, {1,84}
};
@ -2602,7 +2549,7 @@ int main()
SG_bagPositionParam algoParam;
int endGroup = TEST_GROUP - 1;
for (int grp = 15; grp <= 15; grp++)
for (int grp = 14; grp <= 15; grp++)
{
if (grp < 10)
{
@ -2865,17 +2812,19 @@ int main()
int maxTimeStamp = 0;
int clockPerSecond = 0;
sprintf_s(_scan_file, "%s%d-RGB点云.txt", dataPath[grp], fidx);
bool removeNullLines = true;
SVzNLXYZRGBDLaserLine* laser3DPoints = vzReadLaserScanPointFromFile_XYZRGB(
_scan_file,
&lineNum,
&lineV,
&dataCalib,
&maxTimeStamp,
&clockPerSecond);
&clockPerSecond,
removeNullLines);
if (laser3DPoints == NULL)
continue;
algoParam.filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
algoParam.filterParam.continuityTh = 20;// 10.0; // 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
algoParam.filterParam.outlierTh = 5;
long t1 = GetTickCount64();

1
baseAlgorithm/baseAlgorithm.vcxproj
View File

@ -163,6 +163,7 @@
<ItemGroup>
<ClInclude Include="..\sourceCode\SG_baseAlgo_Export.h" />
<ClInclude Include="..\sourceCode\SG_baseDataType.h" />
<ClInclude Include="..\sourceCode\SG_errCode.h" />
</ItemGroup>
<ItemGroup>
<ClCompile Include="..\sourceCode\SG_baseFunc.cpp" />

174
glovePositioning/glovePositioning.vcxproj Normal file
View File

@ -0,0 +1,174 @@
<?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_glovePositioning.cpp" />
</ItemGroup>
<ItemGroup>
<ClInclude Include="..\sourceCode\WD_glovePositioning_Export.h" />
</ItemGroup>
<PropertyGroup Label="Globals">
<VCProjectVersion>16.0</VCProjectVersion>
<Keyword>Win32Proj</Keyword>
<ProjectGuid>{4060ce45-6235-4ca7-b64c-6e4e5cddc34f}</ProjectGuid>
<RootNamespace>glovePositioning</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>
<ExecutablePath>$(VC_ExecutablePath_x64);$(CommonExecutablePath)</ExecutablePath>
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
<LinkIncremental>false</LinkIncremental>
<ExecutablePath>$(VC_ExecutablePath_x64);$(CommonExecutablePath)</ExecutablePath>
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv\build\include;..\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;GLOVEPOSITIONING_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;GLOVEPOSITIONING_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<ConformanceMode>true</ConformanceMode>
<PrecompiledHeader>Use</PrecompiledHeader>
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
</ClCompile>
<Link>
<SubSystem>Windows</SubSystem>
<EnableCOMDATFolding>true</EnableCOMDATFolding>
<OptimizeReferences>true</OptimizeReferences>
<GenerateDebugInformation>true</GenerateDebugInformation>
<EnableUAC>false</EnableUAC>
</Link>
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
<ClCompile>
<WarningLevel>Level3</WarningLevel>
<SDLCheck>true</SDLCheck>
<PreprocessorDefinitions>_DEBUG;GLOVEPOSITIONING_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>NDEBUG;GLOVEPOSITIONING_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>

809
glovePositioning_test/glovePositioning_test.cpp Normal file
View File

@ -0,0 +1,809 @@
// glovePositioning_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//
#include <iostream>
#include <fstream>
#include <vector>
#include <stdio.h>
#include <VZNL_Types.h>
#include "direct.h"
#include <string>
#include "WD_glovePositioning_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 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;
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &X, &Y, &Z);
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 = (int)scanLines.size();
if (lineNum == 0)
return;
sw << "LineNum:" << lineNum << std::endl;
sw << "DataType: 0" << std::endl;
sw << "ScanSpeed: 0" << std::endl;
sw << "PointAdjust: 1" << std::endl;
sw << "MaxTimeStamp: 0_0" << std::endl;
int lineIdx = 0;
int null_lines = 0;
bool counterNull = true;
for (int line = 0; line < lineNum; line++)
{
int linePtNum = (int)scanLines[line].size();
if (linePtNum == 0)
continue;
if (true == removeZeros)
{
int vldPtNum = 0;
for (int i = 0; i < linePtNum; i++)
{
if (scanLines[line][i].pt3D.z > 1e-4)
vldPtNum++;
}
linePtNum = vldPtNum;
}
sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
lineIdx++;
bool isNull = true;
for (int i = 0; i < linePtNum; i++)
{
SVzNL3DPoint* pt3D = &scanLines[line][i].pt3D;
if ((pt3D->z > 1e-4) && (isNull == true))
isNull = false;
if ((true == removeZeros) && (pt3D->z < 1e-4))
continue;
float x = (float)pt3D->x;
float y = (float)pt3D->y;
float z = (float)pt3D->z;
sw << "{ " << x << "," << y << "," << z << " }-";
sw << "{0,0}-{0,0}" << std::endl;
}
if (true == counterNull)
{
if (true == isNull)
null_lines++;
else
counterNull = false;
}
}
*headNullLines = null_lines;
sw.close();
}
//输出水平扫描数据
void _outputScanDataFile_vector_h(char* fileName, std::vector<std::vector<SVzNL3DPosition>>& scanLines)
{
std::ofstream sw(fileName);
int lineNum = (int)scanLines[0].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 linePtNum = (int)scanLines.size();
int lineIdx = 0;
for (int line = 0; line < lineNum; line++)
{
sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
lineIdx++;
bool isNull = true;
for (int i = 0; i < linePtNum; i++)
{
SVzNL3DPoint* pt3D = &scanLines[i][line].pt3D;
float x = (float)pt3D->y;
float y = (float)pt3D->x;
float z = (float)pt3D->z;
sw << "{ " << x << "," << y << "," << z << " }-";
sw << "{0,0}-{0,0}" << std::endl;
}
}
sw.close();
}
void _outputRGBDScanDataFile_RGBD(
char* fileName,
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
std::vector< SSG_peakRgnInfo> objPoints)
{
int lineNum = (int)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 = (int)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_v = pt3D->nPointIdx & 0xffff;
int featureType_h = featureType_v >> 4;
featureType_v &= 0xff;
if (LINE_FEATURE_PEAK_TOP == featureType_v)
{
rgb = { 255, 97, 0 };
size = 5;
}
else if (LINE_FEATURE_PEAK_TOP == featureType_h)
{
rgb = { 97, 255, 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 = (int)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].centerPos.x;
float y = (float)objPoints[i].centerPos.y;
float z = (float)objPoints[i].centerPos.z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
}
//加一个点，用于跳过显示工具bug
float x = (float)objPoints[0].centerPos.x;
float y = (float)objPoints[0].centerPos.y;
float z = (float)objPoints[0].centerPos.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<std::vector< SVzNL3DPosition>>& scanData)
{
std::ifstream inputFile(fileName);
std::string linedata;
if (inputFile.is_open() == false)
return;
std::vector< SVzNL3DPosition> a_line;
int ptIdx = 0;
while (getline(inputFile, linedata))
{
if (0 == strncmp("Line_", linedata.c_str(), 5))
{
int ptSize = (int)a_line.size();
if (ptSize > 0)
{
scanData.push_back(a_line);
}
a_line.clear();
ptIdx = 0;
}
else if (0 == strncmp("{", linedata.c_str(), 1))
{
float X, Y, Z;
int imageY = 0;
float leftX, leftY;
float rightX, rightY;
sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
SVzNL3DPosition a_pt;
a_pt.pt3D.x = X;
a_pt.pt3D.y = Y;
a_pt.pt3D.z = Z;
a_pt.nPointIdx = ptIdx;
ptIdx++;
a_line.push_back(a_pt);
}
}
//last line
int ptSize = (int)a_line.size();
if (ptSize > 0)
{
scanData.push_back(a_line);
a_line.clear();
}
inputFile.close();
return;
}
void _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;
}
void _convertToGridData(std::vector< std::vector<SVzNL3DPosition>>& scanLines,
std::vector< std::vector<SVzNL3DPosition>>& gridScanLines,
double y_step)
{
double y_min = scanLines[0][0].pt3D.y;
double y_max = y_min;
int lineNum = (int)scanLines.size();
for(int line = 0; line < lineNum; line++)
{
std::vector<SVzNL3DPosition>& a_line = scanLines[line];
int ptNum = (int)a_line.size();
for (int i = 0; i < ptNum; i++)
{
if (y_min > a_line[i].pt3D.y)
y_min = a_line[i].pt3D.y;
if (y_max < a_line[i].pt3D.y)
y_max = a_line[i].pt3D.y;
}
}
double half_step = y_step / 2;
int gridPtNum = (int)((y_max - y_min + half_step) / y_step) + 1;
for (int line = 0; line < lineNum; line++)
{
std::vector<SVzNL3DPosition> grid_line;
grid_line.resize(gridPtNum);
for (int i = 0; i < gridPtNum; i++)
grid_line[i] = { 0, {0.0, 0.0, 0.0} };
std::vector<SVzNL3DPosition>& a_line = scanLines[line];
int ptNum = (int)a_line.size();
for (int i = 0; i < ptNum; i++)
{
int ptIdx = (int)((a_line[i].pt3D.y + half_step - y_min) / y_step);
grid_line[ptIdx] = a_line[i];
}
gridScanLines.push_back(grid_line);
}
return;
}
#define CONVERT_TO_GRID 0
#define TEST_COMPUTE_CALIB_PARA 0
#define TEST_COMPUTE_GLOVE_POSITION 1
#define TEST_GROUP 1
int main()
{
const char* dataPath[TEST_GROUP] = {
"F:\\ShangGu\\手套抓取定位\\", //0
};
SVzNLRange fileIdx[TEST_GROUP] = {
{1,35}
};
#if CONVERT_TO_GRID
int convertGrp = 0;
for (int fidx = fileIdx[convertGrp].nMin; fidx <= fileIdx[convertGrp].nMax; fidx++)
{
char _scan_file[256];
sprintf_s(_scan_file, "%sglove_%d.txt", dataPath[convertGrp], fidx);
std::vector< SVzNL3DPoint> scanData;
bool exchangeXY = true;
vzReadLaserScanPointFromFile_plyTxt(_scan_file, scanData, exchangeXY);
//将数据恢复为按扫描线存储格式
std::vector< std::vector<SVzNL3DPosition>> scanLines;
wd_getScanLines(scanData, scanLines);
double y_step;
if ( (fidx == 4)|| (fidx == 5))
y_step = 0.106;
else
y_step = 0.091;
std::vector< std::vector<SVzNL3DPosition>> grid_scanLines;
_convertToGridData(scanLines, grid_scanLines, y_step);
sprintf_s(_scan_file, "%sglove_%d_scanLine.txt", dataPath[convertGrp], fidx);
int headNullLines = 0;
_outputScanDataFile_vector(_scan_file, grid_scanLines, false, &headNullLines);
printf("%s: head null lines = %d\n", _scan_file, headNullLines);
#if 1
sprintf_s(_scan_file, "%sglove_%d_scanLine_h.txt", dataPath[convertGrp], fidx);
_outputScanDataFile_vector_h(_scan_file, grid_scanLines);
#endif
}
#endif
#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_GLOVE_POSITION
for (int grp = 0; grp <= 0; grp++)
{
SSG_planeCalibPara poseCalibPara;
//初始化成单位阵
poseCalibPara.planeCalib[0] = 1.0;
poseCalibPara.planeCalib[1] = 0.0;
poseCalibPara.planeCalib[2] = 0.0;
poseCalibPara.planeCalib[3] = 0.0;
poseCalibPara.planeCalib[4] = 1.0;
poseCalibPara.planeCalib[5] = 0.0;
poseCalibPara.planeCalib[6] = 0.0;
poseCalibPara.planeCalib[7] = 0.0;
poseCalibPara.planeCalib[8] = 1.0;
poseCalibPara.planeHeight = -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, "%sglove_%d_scanLine.txt", dataPath[grp], fidx);
std::vector<std::vector< SVzNL3DPosition>> scanLines;
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
long t1 = (long)GetTickCount64();//统计时间
for (int i = 0, i_max = (int)scanLines.size(); i < i_max; i++)
{
if (i == 14)
int kkk = 1;
//行处理
//调平，去除地面
wd_lineDataR(scanLines[i], poseCalibPara.planeCalib, -1);
}
//
SSG_gloveArcParam arcFeatureParam;
arcFeatureParam.cornerTh = 120; //转角大于150
arcFeatureParam.scale_angle = 1.0;
arcFeatureParam.scale_corner = 5.0;
SSG_treeGrowParam growParam;
growParam.maxLineSkipNum = 5;
growParam.yDeviation_max = 1.0;
growParam.maxSkipDistance = 5.0;
growParam.zDeviation_max = 2;// algoParam.bagParam.bagH / 2; //袋子高度1/2
growParam.minLTypeTreeLen = 30.0; //mm
growParam.minVTypeTreeLen = 30.0; //mm
int errCode = 0;
std::vector<SSG_peakRgnInfo> objOps;
wd_getGloveGrabPostion(
scanLines,
arcFeatureParam,
growParam,
objOps,
&errCode);
long t2 = (long)GetTickCount64();
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
//输出测试结果
sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
_outputRGBDScanDataFile_RGBD(_scan_file, scanLines, objOps);
}
}
#endif
}
// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
// 调试程序: F5 或调试 >“开始调试”菜单
// 入门使用技巧:
// 1. 使用解决方案资源管理器窗口添加/管理文件
// 2. 使用团队资源管理器窗口连接到源代码管理
// 3. 使用输出窗口查看生成输出和其他消息
// 4. 使用错误列表窗口查看错误
// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件

159
glovePositioning_test/glovePositioning_test.vcxproj Normal file
View File

@ -0,0 +1,159 @@
<?xml version="1.0" encoding="utf-8"?>
<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
<ItemGroup Label="ProjectConfigurations">
<ProjectConfiguration Include="Debug|Win32">
<Configuration>Debug</Configuration>
<Platform>Win32</Platform>
</ProjectConfiguration>
<ProjectConfiguration Include="Release|Win32">
<Configuration>Release</Configuration>
<Platform>Win32</Platform>
</ProjectConfiguration>
<ProjectConfiguration Include="Debug|x64">
<Configuration>Debug</Configuration>
<Platform>x64</Platform>
</ProjectConfiguration>
<ProjectConfiguration Include="Release|x64">
<Configuration>Release</Configuration>
<Platform>x64</Platform>
</ProjectConfiguration>
</ItemGroup>
<PropertyGroup Label="Globals">
<VCProjectVersion>16.0</VCProjectVersion>
<Keyword>Win32Proj</Keyword>
<ProjectGuid>{7c1efab7-06cc-4b7d-81bb-64c6fa5996fd}</ProjectGuid>
<RootNamespace>glovePositioningtest</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>
<ExecutablePath>$(VC_ExecutablePath_x64);$(CommonExecutablePath)</ExecutablePath>
<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>
<ExecutablePath>$(VC_ExecutablePath_x64);$(CommonExecutablePath)</ExecutablePath>
<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;glovePositioning.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;glovePositioning.lib;%(AdditionalDependencies)</AdditionalDependencies>
</Link>
</ItemDefinitionGroup>
<ItemGroup>
<ClCompile Include="glovePositioning_test.cpp" />
</ItemGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
<ImportGroup Label="ExtensionTargets">
</ImportGroup>
</Project>

6
sourceCode/SG_baseAlgo_Export.h
View File

@ -60,6 +60,12 @@ SG_APISHARED_EXPORT void sg_getLineCornerFeature(
const SSG_cornerParam cornerPara, //scale通常取bagH的1/4
SSG_lineFeature* line_features);
SG_APISHARED_EXPORT void wd_getLineGloveArcs(
std::vector<SVzNL3DPosition>& lineData,
int lineIdx,
const SSG_gloveArcParam arcPara,
std::vector<SSG_basicFeature1D>& gloveArcs);
/// 提取激光线上的圆柱形特征
SG_APISHARED_EXPORT void sg_getLineUpperSemiCircleFeature(
SVzNL3DPosition* lineData,

23
sourceCode/SG_baseDataType.h
View File

@ -92,6 +92,7 @@ typedef struct
int featureType;
SVzNL2DPoint jumpPos2D;
SVzNL3DPoint jumpPos;
double featureValue;
}SSG_basicFeature1D;
typedef struct
@ -156,6 +157,13 @@ typedef struct
double jumpCornerTh_2;
}SSG_cornerParam;
typedef struct
{
double scale_angle; //计算方向角的窗口比例尺
double scale_corner; //计算方向角转角的窗口比例尺
double cornerTh; //拐角门限，大于此门限，为有效Arc
}SSG_gloveArcParam;
typedef struct
{
double H_len; //直角特征水平段的长度
@ -208,6 +216,7 @@ typedef struct
int treeType;
int sLineIdx;
int eLineIdx;
double tree_value;
SSG_ROIRectD roi;
std::vector< SSG_basicFeature1D> treeNodes;
}SSG_featureTree;
@ -388,4 +397,16 @@ typedef struct
double planeCalib[9]; //旋转矩阵，将点云地面调平
double planeHeight;//地面调平后的高度，用于去除地面数据
double invRMatrix[9]; //旋转矩阵，回到原坐标系
}SSG_planeCalibPara;
}SSG_planeCalibPara;
typedef struct
{
int pntIdx;
double forwardAngle; //前向角
double backwardAngle; //后向角
double corner; //拐角
double forwardDiffZ;
double backwardDiffZ;
double pre_stepDist;
double post_stepDist;
}SSG_pntDirAngle;

9
sourceCode/SG_errCode.h
View File

@ -1,7 +1,8 @@
#pragma once
#define SG_ERR_3D_DATA_INVLD -1000
#define SG_ERR_FOUND_NO_TOP_PLANE -1001
#define SG_ERR_NOT_GRID_FORMAT -1002
#define SG_ERR_LABEL_INFO_ERROR -1003
#define SG_ERR_INVLD_SORTING_MODE -1004
#define SG_ERR_3D_DATA_NULL -1001
#define SG_ERR_FOUND_NO_TOP_PLANE -1002
#define SG_ERR_NOT_GRID_FORMAT -1003
#define SG_ERR_LABEL_INFO_ERROR -1004
#define SG_ERR_INVLD_SORTING_MODE -1005

2
sourceCode/SG_featureGrow.cpp
View File

@ -75,6 +75,7 @@ bool _featureGrowing(SSG_basicFeature1D& a_feature, const int lineIdx, std::vect
{
a_tree.eLineIdx = lineIdx;
a_tree.treeNodes.push_back(a_feature);
a_tree.tree_value += a_feature.featureValue;
return true;
}
}
@ -431,6 +432,7 @@ void sg_lineFeaturesGrowing(
a_newTree.treeType = a_feature.featureType;
a_newTree.sLineIdx = lineIdx;
a_newTree.eLineIdx = lineIdx;
a_newTree.tree_value = a_feature.featureValue;
trees.push_back(a_newTree);
}
}

309
sourceCode/SG_lineFeature.cpp
View File

@ -11,16 +11,6 @@ typedef struct
SVzNL3DPosition endPt;
}SSG_jump;
typedef struct
{
int pntIdx;
double forwardAngle; //前向角
double backwardAngle; //后向角
double corner; //拐角
double forwardDiffZ;
double backwardDiffZ;
}SSG_pntDirAngle;
typedef struct
{
int flag;
@ -1448,22 +1438,18 @@ void sg_getLineCornerFeature(
return;
}
#if 0
void sg_getLineCorners(
//手套的环的特征：在扫描线的Peak点的前后一定范围内，存在前向角从正到负的变化范围超过150度（门限值）
void wd_getLineGloveArcs(
std::vector<SVzNL3DPosition>& lineData,
int lineIdx,
const SSG_cornerParam cornerPara, //scale通常取bagH的1/4
SSG_lineFeature* line_features)
const SSG_gloveArcParam arcPara,
std::vector<SSG_basicFeature1D>& gloveArcs)
{
line_features->lineIdx = lineIdx;
if (lineIdx == 538)
int kkk = 1;
//去除零点
std::vector< SVzNL3DPosition> vldPts;
std::vector<SSG_RUN> segs;
//修改seg的定义。seg端点是两点间距离大于门限的点
int segStart = -1, segEnd = -1;
int dataSize =
int dataSize = (int)lineData.size();
if (dataSize < 10) //点太少，无法检出特征
return;
for (int i = 0; i < dataSize; i++)
{
if ((lineIdx == 399) && (i == 568))
@ -1472,83 +1458,53 @@ void sg_getLineCorners(
a_pt.nPointIdx = i;
if (lineData[i].pt3D.z > 1e-4)
vldPts.push_back(a_pt);
//seg判断
if (lineData[i].pt3D.z > 1e-4)
{
if (segStart < 0)
segStart = i;
else //检查两点距离
{
SVzNL3DPosition pre_pt = lineData[i - 1];
double diff_z = abs(a_pt.pt3D.z - pre_pt.pt3D.z);
if (diff_z > cornerPara.minEndingGap_z)
{
SSG_RUN a_run;
a_run.start = segStart;
a_run.len = segEnd - segStart + 1;
a_run.value = 1;
segs.push_back(a_run);
segStart = i;
}
}
segEnd = i;
}
else
{
if (segStart >= 0)
{
SSG_RUN a_run;
a_run.start = segStart;
a_run.len = segEnd - segStart + 1;
a_run.value = 1;
segs.push_back(a_run);
segStart = -1;
segEnd = -1;
}
}
}
//last
if (segStart >= 0)
{
SSG_RUN a_run;
a_run.start = segStart;
a_run.len = segEnd - segStart + 1;
a_run.value = 1;
segs.push_back(a_run);
}
//计算前向角和后向角
std::vector< SSG_pntDirAngle> corners;
//在同一尺度下计算前向角和后向角，以及极大值点。
std::vector< SSG_pntDirAngle> corners; //逐点计算前向角
corners.resize(vldPts.size());
for (int i = 0, i_max = vldPts.size(); i < i_max; i++)
std::vector< int> localMax; //寻找极大值点
for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++)
{
if ((lineIdx == 399) && (i == 419))
if ((lineIdx == 0) && (i == 2585))
int kkk = 1;
double pre_stepDist = 0;
//前向寻找
bool isMax = true;
int pre_i = -1;
for (int j = i - 1; j >= 0; j--)
{
double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
if (dist >= cornerPara.scale)
if ((true == isMax) &&(vldPts[i].pt3D.z <= vldPts[j].pt3D.z))
isMax = false;
double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
if (j == (i - 1))
pre_stepDist = dist;
if (dist >= arcPara.scale_angle)
{
pre_i = j;
break;
}
}
//后向寻找
double post_stepDist = 0;
int post_i = -1;
for (int j = i + 1; j < i_max; j++)
{
double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
if (dist >= cornerPara.scale)
if (( true == isMax) && (vldPts[i].pt3D.z < vldPts[j].pt3D.z))
isMax = false;
double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
if (j == (i + 1))
post_stepDist = dist;
if (dist >= arcPara.scale_angle)
{
post_i = j;
break;
}
}
if (true == isMax)
localMax.push_back(i);
//计算拐角
if ((pre_i < 0) || (post_i < 0))
{
@ -1558,6 +1514,8 @@ void sg_getLineCorners(
corners[i].corner = 0;
corners[i].forwardDiffZ = 0;
corners[i].backwardDiffZ = 0;
corners[i].pre_stepDist = pre_stepDist;
corners[i].post_stepDist = post_stepDist;
}
else
{
@ -1568,189 +1526,58 @@ void sg_getLineCorners(
corners[i].pntIdx = i;
corners[i].forwardAngle = forwardAngle;
corners[i].backwardAngle = backwardAngle;
corners[i].corner = -(forwardAngle - backwardAngle); //图像坐标系与正常坐标系y方向相反，所以有“-”号
corners[i].corner = (forwardAngle - backwardAngle);
corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z;
corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z;
corners[i].pre_stepDist = pre_stepDist;
corners[i].post_stepDist = post_stepDist;
}
}
//搜索拐角极值
int _state = 0;
int pre_i = -1;
int sEdgePtIdx = -1;
int eEdgePtIdx = -1;
SSG_pntDirAngle* pre_data = NULL;
std::vector< SSG_pntDirAngle> cornerPeakP;
std::vector< SSG_pntDirAngle> cornerPeakM;
for (int i = 0, i_max = vldPts.size(); i < i_max; i++)
//在极大值前后搜索
for (int i = 0, i_max = (int)localMax.size(); i < i_max; i++)
{
if (i == 275)
if (i == 31)
int kkk = 1;
SSG_pntDirAngle* curr_data = &corners[i];
if (curr_data->pntIdx < 0)
int peakIdx = localMax[i];
//向前搜索正的最大前向角
double pre_sumLen = 0;
double pre_angleMax = 0;
for (int j = peakIdx - 1; j >= 0; j--)
{
if (i == i_max - 1) //最后一个
{
if (1 == _state) //上升
{
cornerPeakP.push_back(corners[eEdgePtIdx]);
}
else if (2 == _state) //下降
{
cornerPeakM.push_back(corners[eEdgePtIdx]);
}
}
continue;
}
if (NULL == pre_data)
{
sEdgePtIdx = i;
eEdgePtIdx = i;
pre_data = curr_data;
pre_i = i;
continue;
}
eEdgePtIdx = i;
double cornerDiff = curr_data->corner - pre_data->corner;
switch (_state)
{
case 0: //初态
if (cornerDiff < 0) //下降
{
_state = 2;
}
else if (cornerDiff > 0) //上升
{
_state = 1;
}
break;
case 1: //上升
if (cornerDiff < 0) //下降
{
cornerPeakP.push_back(*pre_data);
_state = 2;
}
break;
case 2: //下降
if (cornerDiff > 0) // 上升
{
cornerPeakM.push_back(*pre_data);
_state = 1;
}
break;
default:
_state = 0;
break;
}
pre_data = curr_data;
pre_i = i;
}
//注意：最后一个不处理，为基座位置
//极小值点（峰顶）
//极值比较，在尺度窗口下寻找局部极值点
double square_distTh = 4 * cornerPara.scale * cornerPara.scale; //2倍的cornerScale。
for (int i = 0, i_max = cornerPeakP.size(); i < i_max; i++)
{
if (cornerPeakP[i].corner < cornerPara.cornerTh)
continue;
bool isPeak = true;
//向前搜索
int cornerPtIdx = cornerPeakP[i].pntIdx;
for (int j = i - 1; j >= 0; j--)
{
int prePtIdx = cornerPeakP[j].pntIdx;
double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ;
if (dist > square_distTh) //超出尺度窗口
if (pre_angleMax < corners[j].forwardAngle)
pre_angleMax = corners[j].forwardAngle;
pre_sumLen += corners[j].post_stepDist;
if (pre_sumLen >= arcPara.scale_corner)
break;
if (cornerPeakP[i].corner < cornerPeakP[j].corner)
{
isPeak = false;
break;
}
}
//向后搜索
if (true == isPeak)
//向后搜索负的最大前向角
double post_angleMax = corners[peakIdx].forwardAngle;
double post_sumLen = corners[peakIdx].post_stepDist;
for (int j = peakIdx + 1; j < (int)vldPts.size(); j++)
{
cornerPtIdx = cornerPeakP[i].pntIdx;
for (int j = i + 1; j < i_max; j++)
{
int postPtIdx = cornerPeakP[j].pntIdx;
double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2);
if (dist > square_distTh) //超出尺度窗口
break;
if (cornerPeakP[i].corner < cornerPeakP[j].corner)
{
isPeak = false;
break;
}
}
post_sumLen += corners[j].post_stepDist;
if (post_sumLen >= arcPara.scale_corner)
break;
if (post_angleMax > corners[j].forwardAngle)
post_angleMax = corners[j].forwardAngle;
}
if (true == isPeak)
//计算最大转角
double totalTurn = pre_angleMax - post_angleMax;
if (totalTurn >= arcPara.cornerTh) //有效ARC
{
SSG_basicFeature1D a_feature;
if ((cornerPeakP[i].backwardAngle > cornerPara.jumpCornerTh_1) && (cornerPeakP[i].forwardAngle > -cornerPara.jumpCornerTh_2))
a_feature.featureType = LINE_FEATURE_L_JUMP_H2L;
else if ((cornerPeakP[i].forwardAngle < -cornerPara.jumpCornerTh_1) && (cornerPeakP[i].backwardAngle < cornerPara.jumpCornerTh_2))
a_feature.featureType = LINE_FEATURE_L_JUMP_L2H;
else
a_feature.featureType = LINE_FEATURE_CORNER_V;
a_feature.jumpPos = vldPts[cornerPtIdx].pt3D;
a_feature.jumpPos2D = { lineIdx, vldPts[cornerPtIdx].nPointIdx };
line_features->features.push_back(a_feature);
a_feature.featureType = LINE_FEATURE_PEAK_TOP;
a_feature.jumpPos = vldPts[peakIdx].pt3D;
a_feature.jumpPos2D = { lineIdx, vldPts[peakIdx].nPointIdx };
a_feature.featureValue = totalTurn;
gloveArcs.push_back(a_feature);
}
}
//添加开始和结束边界
//检查seg是否需要合并；向后合并
for (int i = 0, i_max = segs.size(); i < i_max - 1; i++)
{
SSG_RUN* nxt_seg = &segs[i + 1];
SSG_RUN* curr_seg = &segs[i];
int idx_1 = curr_seg->start + curr_seg->len - 1;
int idx_2 = nxt_seg->start;
double y_diff = abs(lineData[idx_1].pt3D.y - lineData[idx_2].pt3D.y);
double z_diff = abs(lineData[idx_1].pt3D.z - lineData[idx_2].pt3D.z);
if ((y_diff < cornerPara.minEndingGap) && (z_diff < cornerPara.minEndingGap_z)) //合并
{
int idx_end = nxt_seg->start + nxt_seg->len - 1;
nxt_seg->start = curr_seg->start;
nxt_seg->len = idx_end - curr_seg->start + 1;
curr_seg->value = 0;
}
}
for (int i = 0, i_max = segs.size(); i < i_max; i++)
{
if (0 == segs[i].value) //被合并
continue;
int idx_1 = segs[i].start;
int idx_2 = segs[i].start + segs[i].len - 1;
SSG_basicFeature1D an_edge;
memset(&an_edge, 0, sizeof(SSG_basicFeature1D));
an_edge.featureType = LINE_FEATURE_LINE_ENDING_0;
an_edge.jumpPos = lineData[idx_1].pt3D;
an_edge.jumpPos2D = { lineIdx, idx_1 };
line_features->endings.push_back(an_edge);
//line_features.insert(line_features.begin(), an_edge); //头部
//尾部
an_edge.featureType = LINE_FEATURE_LINE_ENDING_1;
an_edge.jumpPos = lineData[idx_2].pt3D;
an_edge.jumpPos2D = { lineIdx, idx_2 };
line_features->endings.push_back(an_edge);
}
return;
}
#endif
/// <summary>
/// 提取激光线上的极值点（极大值点和极小值点）
///

297
sourceCode/WD_glovePositioning.cpp Normal file
View File

@ -0,0 +1,297 @@
#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "WD_glovePositioning_Export.h"
#include <opencv2/opencv.hpp>
#include <limits>
//将ply格式的数据恢复成扫描行的数据形式，从而方面按行进行处理
void wd_getScanLines(
std::vector<SVzNL3DPoint>& scanData,
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
std::vector<SVzNL3DPosition> a_line;
for (int i = 0, i_max = scanData.size(); i < i_max; i++)
{
SVzNL3DPoint a_pt = scanData[i];
SVzNL3DPosition a_idxPt;
a_idxPt.nPointIdx = 0;
a_idxPt.pt3D = a_pt;
if (a_line.size() == 0)
a_line.push_back(a_idxPt);
else
{
SVzNL3DPosition pre_pt = a_line.back();
double x_diff = abs(pre_pt.pt3D.x - a_idxPt.pt3D.x);
if (x_diff > 1e-4)
{
scanLines.push_back(a_line);
a_line.clear();
a_line.push_back(a_idxPt);
}
else
a_line.push_back(a_idxPt);
}
}
//last line
if(a_line.size() > 0)
scanLines.push_back(a_line);
return;
}
void wd_lineDataR(std::vector< SVzNL3DPosition>& a_line,
const double* camPoseR,
double groundH)
{
lineDataRT_vector(a_line, camPoseR, groundH);
}
void wd_getGloveGrabPostion(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSG_gloveArcParam arcPara,
SSG_treeGrowParam growParam,
std::vector<SSG_peakRgnInfo>& objOps,
int* errCode)
{
*errCode = 0;
int lineNum = (int)scanLines.size();
if (lineNum == 0)
{
*errCode = SG_ERR_3D_DATA_NULL;
return;
}
int linePtNum = (int)scanLines[0].size();
bool isGridData = true;
//垂直arc特征提取
std::vector<std::vector<SSG_basicFeature1D>> arcFeatures;
for(int line = 0; line < lineNum; line ++)
{
std::vector<SSG_basicFeature1D> lineArcFeatures;
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
if (linePtNum != (int)lineData.size())
isGridData = false;
wd_getLineGloveArcs(
lineData,
line,
arcPara,
lineArcFeatures);
arcFeatures.push_back(lineArcFeatures);
}
if (false == isGridData)//数据不是网格格式
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return;
}
//生成水平扫描
std::vector<std::vector<SVzNL3DPosition>> hLines;
hLines.resize(linePtNum);
for (int i = 0; i < linePtNum; i++)
hLines[i].resize(lineNum);
for (int line = 0; line < lineNum; line++)
{
for (int j = 0; j < linePtNum; j++)
{
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
hLines[j][line] = scanLines[line][j];
hLines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
hLines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
}
}
//水平arc特征提取
std::vector<std::vector<SSG_basicFeature1D>> arcFeatures_h;
int lineNum_h = (int)hLines.size();
for (int line = 0; line < lineNum_h; line++)
{
std::vector<SSG_basicFeature1D> lineArcFeatures;
std::vector<SVzNL3DPosition>& lineData = hLines[line];
wd_getLineGloveArcs(
lineData,
line,
arcPara,
lineArcFeatures);
arcFeatures_h.push_back(lineArcFeatures);
}
//特征生长
//垂直方向特征生长（激光线方向）
std::vector<SSG_featureTree> v_trees;
for (int line = 0; line < lineNum; line++)
{
bool isLastLine = false;
if (line == lineNum - 1)
isLastLine = true;
std::vector<SSG_basicFeature1D>& a_lineArcFeature = arcFeatures[line];
sg_lineFeaturesGrowing(
line,
isLastLine,
a_lineArcFeature,
v_trees,
growParam);
}
//水平方向特征生长（扫描运动方向）
std::vector<SSG_featureTree> h_trees;
for (int line = 0; line < lineNum_h; line++)
{
if (line == 650)
int kkk = 1;
bool isLastLine = false;
if (line == lineNum_h - 1)
isLastLine = true;
std::vector<SSG_basicFeature1D>& a_lineArcFeature = arcFeatures_h[line];
sg_lineFeaturesGrowing(
line,
isLastLine,
a_lineArcFeature,
h_trees,
growParam);
}
//保留最大平均转角的生长树
double maxTurn_v = 0;
int maxTurn_v_id = -1;
for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
{
int nodeSize = (int)v_trees[i].treeNodes.size();
v_trees[i].tree_value = v_trees[i].tree_value / (double)nodeSize;
if (maxTurn_v_id < 0)
{
maxTurn_v_id = i;
maxTurn_v = v_trees[i].tree_value;
}
else
{
if (maxTurn_v < v_trees[i].tree_value)
{
maxTurn_v_id = i;
maxTurn_v = v_trees[i].tree_value;
}
}
}
double maxTurn_h = 0;
int maxTurn_h_id = -1;
for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++)
{
int nodeSize = (int)h_trees[i].treeNodes.size();
h_trees[i].tree_value = h_trees[i].tree_value / (double)nodeSize;
if (maxTurn_h_id < 0)
{
maxTurn_h_id = i;
maxTurn_h = h_trees[i].tree_value;
}
else
{
if (maxTurn_h < h_trees[i].tree_value)
{
maxTurn_h_id = i;
maxTurn_h = h_trees[i].tree_value;
}
}
}
if ((maxTurn_v_id >= 0) && (maxTurn_h_id < 0))
{
SSG_featureTree bestTree = v_trees[maxTurn_v_id];
v_trees.clear();
v_trees.push_back(bestTree);
}
else if ((maxTurn_v_id < 0) && (maxTurn_h_id >= 0))
{
SSG_featureTree bestTree = h_trees[maxTurn_h_id];
h_trees.clear();
h_trees.push_back(bestTree);
}
else if ((maxTurn_v_id >= 0) && (maxTurn_h_id >= 0))
{
if (maxTurn_h <= maxTurn_v)
{
h_trees.clear();
SSG_featureTree bestTree = v_trees[maxTurn_v_id];
v_trees.clear();
v_trees.push_back(bestTree);
}
else
{
v_trees.clear();
SSG_featureTree bestTree = h_trees[maxTurn_h_id];
h_trees.clear();
h_trees.push_back(bestTree);
}
}
//tree信息
std::vector<SSG_treeInfo> allTreesInfo; //不包含边界
SSG_treeInfo a_nullTree;
memset(&a_nullTree, 0, sizeof(SSG_treeInfo));
allTreesInfo.push_back(a_nullTree); //保持存储位置与treeIdx相同位置，方便索引
//标记，根据起点的生长树进行标注
int hvTreeIdx = 1;
for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
{
SSG_featureTree* a_vTree = &v_trees[i];
//记录Tree的信息
SSG_treeInfo a_treeInfo;
a_treeInfo.vTreeFlag = 1;
a_treeInfo.treeIdx = hvTreeIdx;
a_treeInfo.treeType = a_vTree->treeType;
a_treeInfo.sLineIdx = a_vTree->sLineIdx;
a_treeInfo.eLineIdx = a_vTree->eLineIdx;
a_treeInfo.roi = a_vTree->roi;
allTreesInfo.push_back(a_treeInfo);
//在原始点云上标记，同时有Mask上标记
for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++)
{
SSG_basicFeature1D* a_feature = &a_vTree->treeNodes[j];
if (scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.z > 1e-4)//虚假目标过滤后点会置0
{
int existEdgeId = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx >> 16;
if (existEdgeId == 0)
{
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType;
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx &= 0xffff;
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx += hvTreeIdx << 16;
}
}
}
hvTreeIdx++;
}
int hTreeStart = hvTreeIdx;
////标注:水平特征
for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++)
{
SSG_featureTree* a_hTree = &h_trees[i];
//记录Tree的信息
SSG_treeInfo a_treeInfo;
a_treeInfo.vTreeFlag = 0;
a_treeInfo.treeIdx = hvTreeIdx;
a_treeInfo.treeType = a_hTree->treeType;
a_treeInfo.sLineIdx = a_hTree->sLineIdx;
a_treeInfo.eLineIdx = a_hTree->eLineIdx;
a_treeInfo.roi.left = a_hTree->roi.top; //水平扫描xy是交换的
a_treeInfo.roi.right = a_hTree->roi.bottom;
a_treeInfo.roi.top = a_hTree->roi.left;
a_treeInfo.roi.bottom = a_hTree->roi.right;
allTreesInfo.push_back(a_treeInfo);
//在原始点云上标记，同时有Mask上标记
for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++)
{
SSG_basicFeature1D* a_feature = &a_hTree->treeNodes[j];
if (scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.z > 1e-4)//虚假目标过滤后点会置0
{
int existEdgeId = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx >> 16;
if (existEdgeId == 0)
{
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4;
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx &= 0xffff;
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += hvTreeIdx << 16;
}
}
}
hvTreeIdx++;
}
int hvTreeSize = hvTreeIdx;
}

33
sourceCode/WD_glovePositioning_Export.h Normal file
View File

@ -0,0 +1,33 @@
#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
{
SSG_outlierFilterParam filterParam;
SSG_cornerParam cornerParam;
SSG_treeGrowParam growParam;
}SG_glovePositioningParam;
SG_APISHARED_EXPORT void wd_getScanLines(
std::vector<SVzNL3DPoint>& scanData,
std::vector< std::vector<SVzNL3DPosition>>& scanLines);
SG_APISHARED_EXPORT void wd_lineDataR(std::vector< SVzNL3DPosition>& a_line,
const double* camPoseR,
double groundH);
SG_APISHARED_EXPORT void wd_getGloveGrabPostion(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSG_gloveArcParam arcPara,
SSG_treeGrowParam growParam,
std::vector<SSG_peakRgnInfo>& objOps,
int* errCode);