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

gasFillingPortPosition ver 1.0.0

Browse Source 
加气口定位程序初始版本
This commit is contained in:
jerryzeng 2025-12-16 21:00:17 +08:00
parent 05e5ef143b
commit aa3fd6e49c
11 changed files with 1420 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
SG_Algorithm.sln
View File

@ -139,6 +139,21 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "binocularMarkCam_test", "Bi
{AB25A4FE-6457-4CB4-A190-EBEF603335C7} = {AB25A4FE-6457-4CB4-A190-EBEF603335C7}
EndProjectSection
EndProject
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "gasFillingPortPosition", "gasFillingPortPosition\gasFillingPortPosition.vcxproj", "{5367493C-9799-4684-A809-23B6EF40C70A}"
ProjectSection(ProjectDependencies) = postProject
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
EndProjectSection
EndProject
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "gasFillingPortPosition_test", "gasFillingPortPosition_test\gasFillingPortPosition_test.vcxproj", "{1EF990A1-9CCB-492A-B6CB-5C233300826B}"
ProjectSection(ProjectDependencies) = postProject
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
{5367493C-9799-4684-A809-23B6EF40C70A} = {5367493C-9799-4684-A809-23B6EF40C70A}
EndProjectSection
EndProject
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "BQ_assemblyPosition", "BQ_assemblyPosition\BQ_assemblyPosition.vcxproj", "{0E62EEE4-ABB5-4364-A794-CCFFE8815426}"
EndProject
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "BQ_assemblyPosition_test", "BQ_assemblyPosition_test\BQ_assemblyPosition_test.vcxproj", "{BC38D1E5-10CB-438B-AC72-6012303CE139}"
EndProject
Global
GlobalSection(SolutionConfigurationPlatforms) = preSolution
Debug|x64 = Debug|x64
@ -363,6 +378,38 @@ Global
{FB3BCF92-DF8B-4CEC-AA39-4ECD31F82718}.Release|x64.Build.0 = Release|x64
{FB3BCF92-DF8B-4CEC-AA39-4ECD31F82718}.Release|x86.ActiveCfg = Release|Win32
{FB3BCF92-DF8B-4CEC-AA39-4ECD31F82718}.Release|x86.Build.0 = Release|Win32
{5367493C-9799-4684-A809-23B6EF40C70A}.Debug|x64.ActiveCfg = Debug|x64
{5367493C-9799-4684-A809-23B6EF40C70A}.Debug|x64.Build.0 = Debug|x64
{5367493C-9799-4684-A809-23B6EF40C70A}.Debug|x86.ActiveCfg = Debug|Win32
{5367493C-9799-4684-A809-23B6EF40C70A}.Debug|x86.Build.0 = Debug|Win32
{5367493C-9799-4684-A809-23B6EF40C70A}.Release|x64.ActiveCfg = Release|x64
{5367493C-9799-4684-A809-23B6EF40C70A}.Release|x64.Build.0 = Release|x64
{5367493C-9799-4684-A809-23B6EF40C70A}.Release|x86.ActiveCfg = Release|Win32
{5367493C-9799-4684-A809-23B6EF40C70A}.Release|x86.Build.0 = Release|Win32
{1EF990A1-9CCB-492A-B6CB-5C233300826B}.Debug|x64.ActiveCfg = Debug|x64
{1EF990A1-9CCB-492A-B6CB-5C233300826B}.Debug|x64.Build.0 = Debug|x64
{1EF990A1-9CCB-492A-B6CB-5C233300826B}.Debug|x86.ActiveCfg = Debug|Win32
{1EF990A1-9CCB-492A-B6CB-5C233300826B}.Debug|x86.Build.0 = Debug|Win32
{1EF990A1-9CCB-492A-B6CB-5C233300826B}.Release|x64.ActiveCfg = Release|x64
{1EF990A1-9CCB-492A-B6CB-5C233300826B}.Release|x64.Build.0 = Release|x64
{1EF990A1-9CCB-492A-B6CB-5C233300826B}.Release|x86.ActiveCfg = Release|Win32
{1EF990A1-9CCB-492A-B6CB-5C233300826B}.Release|x86.Build.0 = Release|Win32
{0E62EEE4-ABB5-4364-A794-CCFFE8815426}.Debug|x64.ActiveCfg = Debug|x64
{0E62EEE4-ABB5-4364-A794-CCFFE8815426}.Debug|x64.Build.0 = Debug|x64
{0E62EEE4-ABB5-4364-A794-CCFFE8815426}.Debug|x86.ActiveCfg = Debug|Win32
{0E62EEE4-ABB5-4364-A794-CCFFE8815426}.Debug|x86.Build.0 = Debug|Win32
{0E62EEE4-ABB5-4364-A794-CCFFE8815426}.Release|x64.ActiveCfg = Release|x64
{0E62EEE4-ABB5-4364-A794-CCFFE8815426}.Release|x64.Build.0 = Release|x64
{0E62EEE4-ABB5-4364-A794-CCFFE8815426}.Release|x86.ActiveCfg = Release|Win32
{0E62EEE4-ABB5-4364-A794-CCFFE8815426}.Release|x86.Build.0 = Release|Win32
{BC38D1E5-10CB-438B-AC72-6012303CE139}.Debug|x64.ActiveCfg = Debug|x64
{BC38D1E5-10CB-438B-AC72-6012303CE139}.Debug|x64.Build.0 = Debug|x64
{BC38D1E5-10CB-438B-AC72-6012303CE139}.Debug|x86.ActiveCfg = Debug|Win32
{BC38D1E5-10CB-438B-AC72-6012303CE139}.Debug|x86.Build.0 = Debug|Win32
{BC38D1E5-10CB-438B-AC72-6012303CE139}.Release|x64.ActiveCfg = Release|x64
{BC38D1E5-10CB-438B-AC72-6012303CE139}.Release|x64.Build.0 = Release|x64
{BC38D1E5-10CB-438B-AC72-6012303CE139}.Release|x86.ActiveCfg = Release|Win32
{BC38D1E5-10CB-438B-AC72-6012303CE139}.Release|x86.Build.0 = Release|Win32
EndGlobalSection
GlobalSection(SolutionProperties) = preSolution
HideSolutionNode = FALSE

172
gasFillingPortPosition/gasFillingPortPosition.vcxproj Normal file
View File

@ -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\gasFillingPortPosition.cpp" />
</ItemGroup>
<ItemGroup>
<ClInclude Include="..\sourceCode\gasFillingPortPosition_Export.h" />
</ItemGroup>
<PropertyGroup Label="Globals">
<VCProjectVersion>16.0</VCProjectVersion>
<Keyword>Win32Proj</Keyword>
<ProjectGuid>{5367493c-9799-4684-a809-23b6ef40c70a}</ProjectGuid>
<RootNamespace>gasFillingPortPosition</RootNamespace>
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
</PropertyGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
<ConfigurationType>DynamicLibrary</ConfigurationType>
<UseDebugLibraries>true</UseDebugLibraries>
<PlatformToolset>v142</PlatformToolset>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
<ConfigurationType>DynamicLibrary</ConfigurationType>
<UseDebugLibraries>false</UseDebugLibraries>
<PlatformToolset>v142</PlatformToolset>
<WholeProgramOptimization>true</WholeProgramOptimization>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
<ConfigurationType>DynamicLibrary</ConfigurationType>
<UseDebugLibraries>true</UseDebugLibraries>
<PlatformToolset>v142</PlatformToolset>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
<ConfigurationType>DynamicLibrary</ConfigurationType>
<UseDebugLibraries>false</UseDebugLibraries>
<PlatformToolset>v142</PlatformToolset>
<WholeProgramOptimization>true</WholeProgramOptimization>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
<ImportGroup Label="ExtensionSettings">
</ImportGroup>
<ImportGroup Label="Shared">
</ImportGroup>
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
</ImportGroup>
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
</ImportGroup>
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
</ImportGroup>
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
</ImportGroup>
<PropertyGroup Label="UserMacros" />
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
<LinkIncremental>true</LinkIncremental>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
<LinkIncremental>false</LinkIncremental>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
<LinkIncremental>true</LinkIncremental>
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
<LinkIncremental>false</LinkIncremental>
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
</PropertyGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
<ClCompile>
<WarningLevel>Level3</WarningLevel>
<SDLCheck>true</SDLCheck>
<PreprocessorDefinitions>WIN32;_DEBUG;GASFILLINGPORTPOSITION_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;GASFILLINGPORTPOSITION_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>_CRT_SECURE_NO_WARNINGS;_DEBUG;GASFILLINGPORTPOSITION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<ConformanceMode>true</ConformanceMode>
<PrecompiledHeader>NotUsing</PrecompiledHeader>
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
</ClCompile>
<Link>
<SubSystem>Windows</SubSystem>
<GenerateDebugInformation>true</GenerateDebugInformation>
<EnableUAC>false</EnableUAC>
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
<AdditionalDependencies>opencv_world320d.lib;baseAlgorithm.lib;%(AdditionalDependencies)</AdditionalDependencies>
</Link>
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
<ClCompile>
<WarningLevel>Level3</WarningLevel>
<FunctionLevelLinking>true</FunctionLevelLinking>
<IntrinsicFunctions>true</IntrinsicFunctions>
<SDLCheck>true</SDLCheck>
<PreprocessorDefinitions>_CRT_SECURE_NO_WARNINGS;NDEBUG;GASFILLINGPORTPOSITION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<ConformanceMode>true</ConformanceMode>
<PrecompiledHeader>NotUsing</PrecompiledHeader>
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
</ClCompile>
<Link>
<SubSystem>Windows</SubSystem>
<EnableCOMDATFolding>true</EnableCOMDATFolding>
<OptimizeReferences>true</OptimizeReferences>
<GenerateDebugInformation>true</GenerateDebugInformation>
<EnableUAC>false</EnableUAC>
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
<AdditionalDependencies>opencv_world320.lib;baseAlgorithm.lib;%(AdditionalDependencies)</AdditionalDependencies>
</Link>
</ItemDefinitionGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
<ImportGroup Label="ExtensionTargets">
</ImportGroup>
</Project>

293
gasFillingPortPosition_test/gasFillingPortPosition_test.cpp Normal file
View File

@ -0,0 +1,293 @@
// gasFillingPortPosition_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//
#include <iostream>
#include <fstream>
#include <vector>
#include <stdio.h>
#include <VZNL_Types.h>
#include "direct.h"
#include <string>
#include "gasFillingPortPosition_Export.h"
#include <opencv2/opencv.hpp>
#include <Windows.h>
#include <limits>
typedef struct
{
int r;
int g;
int b;
}SG_color;
typedef struct
{
int nPointIdx;
double x;
double y;
double z;
float r;
float g;
float b;
} SPointXYZRGB;
void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData)
{
std::ifstream inputFile(fileName);
std::string linedata;
if (inputFile.is_open() == false)
return;
std::vector< SVzNL3DPosition> a_line;
int ptIdx = 0;
while (getline(inputFile, linedata))
{
if (0 == strncmp("Line_", linedata.c_str(), 5))
{
int ptSize = (int)a_line.size();
if (ptSize > 0)
{
scanData.push_back(a_line);
}
a_line.clear();
ptIdx = 0;
}
else if (0 == strncmp("{", linedata.c_str(), 1))
{
float X, Y, Z;
int imageY = 0;
float leftX, leftY;
float rightX, rightY;
sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
SVzNL3DPosition a_pt;
a_pt.pt3D.x = X;
a_pt.pt3D.y = Y;
a_pt.pt3D.z = Z;
a_pt.nPointIdx = ptIdx;
ptIdx++;
a_line.push_back(a_pt);
}
}
//last line
int ptSize = (int)a_line.size();
if (ptSize > 0)
{
scanData.push_back(a_line);
a_line.clear();
}
inputFile.close();
return;
}
void _outputFillingPortInfo(char* fileName, SSG_6DOF centerPose)
{
std::ofstream sw(fileName);
char dataStr[250];
sw << "A:" << std::endl;
sprintf_s(dataStr, 250, "中心点: (%g, %g, %g)", centerPose.x, centerPose.y, centerPose.z);
sw << dataStr << std::endl;
sprintf_s(dataStr, 250, "法向量: (%g, %g, %g)", centerPose.x_roll, centerPose.y_pitch, centerPose.z_yaw);
sw << dataStr << std::endl;
sw.close();
}
void _outputRGBDScan_fillingPort_RGBD(
char* fileName,
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
SSG_6DOF centerPose)
{
int lineNum = (int)scanLines.size();
std::ofstream sw(fileName);
int realLines = lineNum;
if (centerPose.z > 1e-4)
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];
if (pt3D->nPointIdx > 0)
int kkk = 1;
if(pt3D->nPointIdx == 0)
{
rgb = { 200, 200, 200 };
size = 1;
}
else
{
rgb = { 255, 97, 0 };
size = 3;
}
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 (centerPose.z > 1e-4)
{
std::vector<SVzNL3DPoint> ptBuffer;
SVzNL3DPoint cpt = { centerPose.x, centerPose.y, centerPose.z };
ptBuffer.push_back(cpt);
int linePtNum = (int)ptBuffer.size();
sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl;
lineNum++;
rgb = { 255, 0, 0 };
size = 15;
for (int j = 0; j < linePtNum; j++)
{
float x = (float)ptBuffer[j].x;
float y = (float)ptBuffer[j].y;
float z = (float)ptBuffer[j].z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
}
//加一个点，用于跳过显示工具bug
float x = (float)ptBuffer[0].x;
float y = (float)ptBuffer[0].y;
float z = (float)ptBuffer[0].z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
size = 1;
double len = 60;
SVzNL3DPoint pt0 = { cpt.x + len * centerPose.x_roll,
cpt.y + len * centerPose.y_pitch,
cpt.z + len * centerPose.z_yaw };
SVzNL3DPoint pt1 = { cpt.x - len * centerPose.x_roll,
cpt.y - len * centerPose.y_pitch,
cpt.z - len * centerPose.z_yaw };
//显示法向量
sw << "Poly_" << lineIdx << "_2" << std::endl;
sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
sw << "{" << pt1.x << "," << pt1.y << "," << pt1.z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
lineIdx++;
}
sw.close();
}
#define TEST_GROUP 1
int main()
{
const char* dataPath[TEST_GROUP] = {
"F:/ShangGu/项目/冠钦_LNG自动加气/加气口测试数据/", //0
};
SVzNLRange fileIdx[TEST_GROUP] = {
{2,30},
};
const char* ver = wd_gasFillingPortPositionVersion();
printf("ver:%s\n", ver);
for (int grp = 0; grp < TEST_GROUP; grp++)
{
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
{
//fidx =4;
char _scan_file[256];
sprintf_s(_scan_file, "%s%d_LaserData_Hi229156.txt", dataPath[grp], fidx);
std::vector<std::vector< SVzNL3DPosition>> scanLines;
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
//转成plyTxt格式
//sprintf_s(_scan_file, "%s%d_ply_Hi229229.txt", dataPath[grp], fidx);
//wdSavePlyTxt(_scan_file, scanLines);
long t1 = (long)GetTickCount64();//统计时间
SSX_gasFillingPortPara gasFillingPortPara;
gasFillingPortPara.innerD = 42.0;
gasFillingPortPara.outerD = 52.0;
SSG_lineSegParam lineSegPara;
lineSegPara.maxDist = 1.0;
lineSegPara.segGapTh_y = 5.0; //y方向间隔大于5mm认为是分段
lineSegPara.segGapTh_z = 10.0; //z方向间隔大于10mm认为是分段
SSG_outlierFilterParam filterParam;
filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
filterParam.outlierTh = 5;
SSG_treeGrowParam growParam;
growParam.maxLineSkipNum = 10;
growParam.yDeviation_max = 5.0;
growParam.maxSkipDistance = 5.0;
growParam.zDeviation_max = 2.0;//
growParam.minLTypeTreeLen = 100; //mm
growParam.minVTypeTreeLen = 100; //mm
int errCode = 0;
SSG_6DOF centerPose = wd_getGasFillingPortPosition(
scanLines,
gasFillingPortPara,
lineSegPara,
filterParam,
growParam,
&errCode);
long t2 = (long)GetTickCount64();
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
//输出测试结果
sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx);
_outputRGBDScan_fillingPort_RGBD(_scan_file, scanLines, centerPose);
sprintf_s(_scan_file, "%sresult\\%d_fillingPort_info.txt", dataPath[grp], fidx);
_outputFillingPortInfo(_scan_file, centerPose);
}
}
}
// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
// 调试程序: F5 或调试 >“开始调试”菜单
// 入门使用技巧:
// 1. 使用解决方案资源管理器窗口添加/管理文件
// 2. 使用团队资源管理器窗口连接到源代码管理
// 3. 使用输出窗口查看生成输出和其他消息
// 4. 使用错误列表窗口查看错误
// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件

157
gasFillingPortPosition_test/gasFillingPortPosition_test.vcxproj Normal file
View File

@ -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>{1ef990a1-9ccb-492a-b6cb-5c233300826b}</ProjectGuid>
<RootNamespace>gasFillingPortPositiontest</RootNamespace>
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
</PropertyGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
<ConfigurationType>Application</ConfigurationType>
<UseDebugLibraries>true</UseDebugLibraries>
<PlatformToolset>v142</PlatformToolset>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
<ConfigurationType>Application</ConfigurationType>
<UseDebugLibraries>false</UseDebugLibraries>
<PlatformToolset>v142</PlatformToolset>
<WholeProgramOptimization>true</WholeProgramOptimization>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
<ConfigurationType>Application</ConfigurationType>
<UseDebugLibraries>true</UseDebugLibraries>
<PlatformToolset>v142</PlatformToolset>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
<ConfigurationType>Application</ConfigurationType>
<UseDebugLibraries>false</UseDebugLibraries>
<PlatformToolset>v142</PlatformToolset>
<WholeProgramOptimization>true</WholeProgramOptimization>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
<ImportGroup Label="ExtensionSettings">
</ImportGroup>
<ImportGroup Label="Shared">
</ImportGroup>
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
</ImportGroup>
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
</ImportGroup>
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
</ImportGroup>
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
</ImportGroup>
<PropertyGroup Label="UserMacros" />
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
<LinkIncremental>true</LinkIncremental>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
<LinkIncremental>false</LinkIncremental>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
<LinkIncremental>true</LinkIncremental>
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
<LinkIncremental>false</LinkIncremental>
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
</PropertyGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
<ClCompile>
<WarningLevel>Level3</WarningLevel>
<SDLCheck>true</SDLCheck>
<PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<ConformanceMode>true</ConformanceMode>
</ClCompile>
<Link>
<SubSystem>Console</SubSystem>
<GenerateDebugInformation>true</GenerateDebugInformation>
</Link>
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
<ClCompile>
<WarningLevel>Level3</WarningLevel>
<FunctionLevelLinking>true</FunctionLevelLinking>
<IntrinsicFunctions>true</IntrinsicFunctions>
<SDLCheck>true</SDLCheck>
<PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<ConformanceMode>true</ConformanceMode>
</ClCompile>
<Link>
<SubSystem>Console</SubSystem>
<EnableCOMDATFolding>true</EnableCOMDATFolding>
<OptimizeReferences>true</OptimizeReferences>
<GenerateDebugInformation>true</GenerateDebugInformation>
</Link>
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
<ClCompile>
<WarningLevel>Level3</WarningLevel>
<SDLCheck>true</SDLCheck>
<PreprocessorDefinitions>_CRT_SECURE_NO_WARNINGS;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<ConformanceMode>true</ConformanceMode>
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;</AdditionalIncludeDirectories>
</ClCompile>
<Link>
<SubSystem>Console</SubSystem>
<GenerateDebugInformation>true</GenerateDebugInformation>
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
<AdditionalDependencies>opencv_world320d.lib;gasFillingPortPosition.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>_CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<ConformanceMode>true</ConformanceMode>
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;</AdditionalIncludeDirectories>
</ClCompile>
<Link>
<SubSystem>Console</SubSystem>
<EnableCOMDATFolding>true</EnableCOMDATFolding>
<OptimizeReferences>true</OptimizeReferences>
<GenerateDebugInformation>true</GenerateDebugInformation>
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
<AdditionalDependencies>opencv_world320.lib;gasFillingPortPosition.lib;%(AdditionalDependencies)</AdditionalDependencies>
</Link>
</ItemDefinitionGroup>
<ItemGroup>
<ClCompile Include="gasFillingPortPosition_test.cpp" />
</ItemGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
<ImportGroup Label="ExtensionTargets">
</ImportGroup>
</Project>

38
sourceCode/SG_baseAlgo_Export.h
View File

@ -102,6 +102,14 @@ SG_APISHARED_EXPORT void wd_getRingArcFeature(
std::vector<SWD_segFeature>& line_ringArcs //环
);
//直线特征提取：对split-and-merge法作了简化，以起点终点直线代替拟合直线
SG_APISHARED_EXPORT void wd_simpleLineSegment(
std::vector< SVzNL3DPosition>& lineData,
int lineIdx,
SVzNLRangeD lineLenRange,
const SSG_lineSegParam lineSegPara,
std::vector<SSG_featureSemiCircle>& lineSegs);
/// <summary>
/// 提取激光线上的Jumping特征
/// nPointIdx被重新定义成Feature类型
@ -345,6 +353,12 @@ SG_APISHARED_EXPORT void lineFitting_abc(
double* _b,
double* _c);
//圆最小二乘拟合
SG_APISHARED_EXPORT double fitCircleByLeastSquare(
const std::vector<SVzNL3DPoint>& pointArray,
SVzNL3DPoint& center,
double& radius);
//计算Z均值
SG_APISHARED_EXPORT double computeMeanZ(std::vector< SVzNL3DPoint>& pts);
@ -412,6 +426,12 @@ SG_APISHARED_EXPORT void SG_TwoPassLabel(
std::vector<SSG_Region>& labelRgns,
int connectivity = 4);
//计算面参数: z = Ax + By + C
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
SG_APISHARED_EXPORT void vzCaculateLaserPlane(
std::vector<cv::Point3f> Points3ds,
std::vector<double>& res);
//计算一个平面调平参数。
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
@ -419,6 +439,11 @@ SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara(
SVzNL3DLaserLine* laser3DPoints,
int lineNum);
//将一个平面调整为水平平面(z为固定值），
SG_APISHARED_EXPORT SSG_planeCalibPara adjustPlaneToXYPlane(
double plane_A, double plane_B, double plane_C //平面法向量
);
//计算一个平面调平参数。
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
@ -460,12 +485,25 @@ SG_APISHARED_EXPORT void lineDataRT_RGBD(
const double* camPoseR,
double groundH);
//基于相邻点的聚类
SG_APISHARED_EXPORT void sg_pointClustering(
std::vector< SVzNL3DPosition>& pts,
double clusterDist,
std::vector<std::vector< SVzNL3DPosition>>& objClusters //result
);
//基于栅格上点的窗口内的相邻点的聚类，聚类条件由3D点的邻域决定
//使用vector构成2维结构体数组
SG_APISHARED_EXPORT void wd_pointClustering2D(
std::vector<std::vector<SSG_featureClusteringInfo>>& featureMask,
std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
int clusterCheckWin, //搜索窗口
SSG_treeGrowParam growParam,//聚类条件
int clusterID, //当前Cluster的ID
std::vector< SVzNL2DPoint>& a_cluster, //result
SVzNL3DRangeD& clusterRoi //roi3D
);
//对栅格化数据进行XY平面上的投影量化，并对量化产生的空白点进行插值
SG_APISHARED_EXPORT void pointClout2DProjection(
std::vector< std::vector<SVzNL3DPosition>>& gridScanData,

18
sourceCode/SG_baseDataType.h
View File

@ -48,6 +48,17 @@ typedef struct
int idx;
}SSG_intPair;
typedef struct
{
int featurType;
int featureIdx_v;
int featureIdx_h;
int clusterID;
int flag;
int lineIdx;
int ptIdx;
}SSG_featureClusteringInfo;
typedef struct
{
double left;
@ -157,6 +168,13 @@ typedef struct
double jumpCornerTh_2;
}SSG_cornerParam;
typedef struct
{
double segGapTh_y; //y方向连续段门限。大于此门限，为不连续
double segGapTh_z; //z方向连续段门限。大于此门限，为不连续
double maxDist; //计算方向角的窗口比例尺
}SSG_lineSegParam;
typedef struct
{
double scale_angle; //计算方向角的窗口比例尺

107
sourceCode/SG_baseFunc.cpp
View File

@ -279,6 +279,72 @@ void lineFitting_abc(std::vector< SVzNL3DPoint>& inliers, double* _a, double* _b
return;
}
//圆最小二乘拟合
double fitCircleByLeastSquare(
const std::vector<SVzNL3DPoint>& pointArray,
SVzNL3DPoint& center,
double& radius)
{
int N = pointArray.size();
if (N < 3) {
return std::numeric_limits<double>::max();
}
double sumX = 0.0;
double sumY = 0.0;
double sumX2 = 0.0;
double sumY2 = 0.0;
double sumX3 = 0.0;
double sumY3 = 0.0;
double sumXY = 0.0;
double sumXY2 = 0.0;
double sumX2Y = 0.0;
for (int pId = 0; pId < N; ++pId) {
sumX += pointArray[pId].x;
sumY += pointArray[pId].y;
double x2 = pointArray[pId].x * pointArray[pId].x;
double y2 = pointArray[pId].y * pointArray[pId].y;
sumX2 += x2;
sumY2 += y2;
sumX3 += x2 * pointArray[pId].x;
sumY3 += y2 * pointArray[pId].y;
sumXY += pointArray[pId].x * pointArray[pId].y;
sumXY2 += pointArray[pId].x * y2;
sumX2Y += x2 * pointArray[pId].y;
}
double C, D, E, G, H;
double a, b, c;
C = N * sumX2 - sumX * sumX;
D = N * sumXY - sumX * sumY;
E = N * sumX3 + N * sumXY2 - (sumX2 + sumY2) * sumX;
G = N * sumY2 - sumY * sumY;
H = N * sumX2Y + N * sumY3 - (sumX2 + sumY2) * sumY;
a = (H * D - E * G) / (C * G - D * D);
b = (H * C - E * D) / (D * D - G * C);
c = -(a * sumX + b * sumY + sumX2 + sumY2) / N;
center.x = -a / 2.0;
center.y = -b / 2.0;
radius = sqrt(a * a + b * b - 4 * c) / 2.0;
double err = 0.0;
double e;
double r2 = radius * radius;
for (int pId = 0; pId < N; ++pId) {
e = pow(pointArray[pId].x - center.x,2) + pow(pointArray[pId].y - center.y, 2) - r2;
if (e > err) {
err = e;
}
}
return err;
}
//计算Z均值
double computeMeanZ(std::vector< SVzNL3DPoint>& pts)
{
@ -1357,6 +1423,7 @@ SSG_planeCalibPara sg_getPlaneCalibPara(
}
//平面拟合
std::vector<double> planceFunc;
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
vzCaculateLaserPlane(Points3ds, planceFunc);
#if 1 //两个向量的旋转旋转，使用四元数法，
@ -1453,6 +1520,46 @@ SSG_planeCalibPara sg_getPlaneCalibPara(
return planePara;
}
SSG_planeCalibPara adjustPlaneToXYPlane(double plane_A, double plane_B, double plane_C)
{
SSG_planeCalibPara calibPara;
//两个向量的旋转旋转，使用四元数法，
Vector3 a = Vector3(plane_A, plane_B, plane_C);
Vector3 b = Vector3(0, 0, -1.0);
Quaternion quanPara = rotationBetweenVectors(a, b);
RotationMatrix rMatrix;
quaternionToMatrix(quanPara, rMatrix.data);
//计算反向旋转矩阵
Quaternion invQuanPara = rotationBetweenVectors(b, a);
RotationMatrix invMatrix;
quaternionToMatrix(invQuanPara, invMatrix.data);
calibPara.planeCalib[0] = rMatrix.data[0][0];
calibPara.planeCalib[1] = rMatrix.data[0][1];
calibPara.planeCalib[2] = rMatrix.data[0][2];
calibPara.planeCalib[3] = rMatrix.data[1][0];
calibPara.planeCalib[4] = rMatrix.data[1][1];
calibPara.planeCalib[5] = rMatrix.data[1][2];
calibPara.planeCalib[6] = rMatrix.data[2][0];
calibPara.planeCalib[7] = rMatrix.data[2][1];
calibPara.planeCalib[8] = rMatrix.data[2][2];
calibPara.invRMatrix[0] = invMatrix.data[0][0];
calibPara.invRMatrix[1] = invMatrix.data[0][1];
calibPara.invRMatrix[2] = invMatrix.data[0][2];
calibPara.invRMatrix[3] = invMatrix.data[1][0];
calibPara.invRMatrix[4] = invMatrix.data[1][1];
calibPara.invRMatrix[5] = invMatrix.data[1][2];
calibPara.invRMatrix[6] = invMatrix.data[2][0];
calibPara.invRMatrix[7] = invMatrix.data[2][1];
calibPara.invRMatrix[8] = invMatrix.data[2][2];
return calibPara;
}
//计算一个平面调平参数。
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数

74
sourceCode/SG_clustering.cpp
View File

@ -66,4 +66,76 @@ void sg_pointClustering(
}
return;
}
;
void wd_pointClustering2D(
std::vector<std::vector<SSG_featureClusteringInfo>>& featureMask,
std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
int clusterCheckWin, //搜索窗口
SSG_treeGrowParam growParam,//聚类条件
int clusterID, //当前Cluster的ID
std::vector< SVzNL2DPoint>& a_cluster, //result
SVzNL3DRangeD& clusterRoi //roi3D
)
{
int i = 0;
int lineNum = (int)featureMask.size();
int linePtNum = (int)featureMask[0].size();
while (1)
{
if (i >= a_cluster.size())
break;
SVzNL2DPoint a_seedPos = a_cluster[i];
if ((a_seedPos.x == 390) && (a_seedPos.y == 949))
int kkk = 1;
SSG_featureClusteringInfo& a_seed = featureMask[a_seedPos.x][a_seedPos.y];
SVzNL3DPoint& seedValue = feature3DInfo[a_seedPos.x][a_seedPos.y];
if (0 == a_seed.clusterID) //clusterID == 0, 未被处理，搜索邻域
{
for (int i = -clusterCheckWin; i <= clusterCheckWin; i++)
{
for (int j = -clusterCheckWin; j <= clusterCheckWin; j++)
{
int y = j + a_seedPos.y;
int x = i + a_seedPos.x;
if ((x == 390) && (y == 949))
int kkk = 1;
if ((x >= 0) && (x < lineNum) && (y >= 0) && (y < linePtNum))
{
SSG_featureClusteringInfo& chk_seed = featureMask[x][y];
if ((chk_seed.featurType ==0) || (chk_seed.clusterID > 0)) //只检查未聚类的特征点
continue;
SVzNL3DPoint& chkValue = feature3DInfo[x][y];;
double y_diff = abs(seedValue.y - chkValue.y);
double z_diff = abs(seedValue.z - chkValue.z);
double x_diff = abs(seedValue.x - chkValue.x);
if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) &&
(x_diff < growParam.maxSkipDistance))
{
if (0 == chk_seed.flag)//防止被重复添加
{
chk_seed.flag = 1;
SVzNL2DPoint new_seed = { x, y };
a_cluster.push_back(new_seed);
}
}
}
}
}
}
a_seed.clusterID = clusterID;
//更新ROI
clusterRoi.xRange.min = clusterRoi.xRange.min > seedValue.x ? seedValue.x : clusterRoi.xRange.min;
clusterRoi.xRange.max = clusterRoi.xRange.max < seedValue.x ? seedValue.x : clusterRoi.xRange.max;
clusterRoi.yRange.min = clusterRoi.yRange.min > seedValue.y ? seedValue.y : clusterRoi.yRange.min;
clusterRoi.yRange.max = clusterRoi.yRange.max < seedValue.y ? seedValue.y : clusterRoi.yRange.max;
clusterRoi.zRange.min = clusterRoi.zRange.min > seedValue.z ? seedValue.z : clusterRoi.zRange.min;
clusterRoi.zRange.max = clusterRoi.zRange.max < seedValue.z ? seedValue.z : clusterRoi.zRange.max;
i++;
}
return;
}

164
sourceCode/SG_lineFeature.cpp
View File

@ -2558,6 +2558,170 @@ void wd_getRingArcFeature(
return;
}
// 最小点集数量（小于此数无法拟合直线）
const int MIN_POINT_COUNT = 5;
//使用端点直线，检查点到直线的距离，大于门限的分割
void split(
SSG_RUN a_run,
std::vector< SVzNL3DPosition>& lineData,
const double maxError,
std::vector< SSG_RUN>& lineSegs)
{
if (a_run.len < MIN_POINT_COUNT)
return;
int start = a_run.start;
int end = a_run.start + a_run.len - 1;
SVzNL3DPoint pt1 = lineData[start].pt3D;
SVzNL3DPoint pt2 = lineData[end].pt3D;
if ((pt1.z < 1e-4) || (pt2.z < 1e-4))
return;
double _a, _b, _c;
compute2ptLine(pt1, pt2, &_a, &_b, &_c);
double denominator = sqrt(_a * _a + _b * _b);
//归一化
_a = _a / denominator;
_b = _b / denominator;
_c = _c / denominator;
double maxDist = 0;
double maxPos = 0;
for (int i = start; i <= end; i++)
{
SVzNL3DPoint a_pt = lineData[i].pt3D;
if (a_pt.z > 1e-4)
{
double dist = abs(a_pt.x * _a + a_pt.y * _b + _c);
if (maxDist < dist)
{
maxDist = dist;
maxPos = i;
}
}
}
if (maxDist < maxError)
{
// 误差满足要求，加入线段集合
lineSegs.push_back(a_run);
return;
}
SSG_RUN run_1 = { a_run.start, maxPos-a_run.start + 1, 0 };
SSG_RUN run_2 = { maxPos, a_run.start + a_run.len - maxPos, 0 };
split(run_1, lineData, maxError, lineSegs);
split(run_2, lineData, maxError, lineSegs);
}
//直线特征提取：对split-and-merge法作了简化，以起点终点直线代替拟合直线
void wd_simpleLineSegment(
std::vector< SVzNL3DPosition>& lineData,
int lineIdx,
SVzNLRangeD lineLenRange,
const SSG_lineSegParam lineSegPara,
std::vector<SSG_featureSemiCircle>&lineSegs)
{
int dataSize = (int)lineData.size();
//去除零点
std::vector<SSG_RUN> segs;
int runIdx = 1;
SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx
double pre_z = 0;
double pre_y = 0;
for (int i = 0; i < dataSize; i++)
{
if (lineData[i].pt3D.z > 1e-4)
{
if (a_run.len < 0)
{
a_run.start = i;
a_run.len = 1;
a_run.value = i;
}
else
{
double z_diff = abs(lineData[i].pt3D.z - pre_z);
double y_diff = abs(lineData[i].pt3D.y - pre_y);
if ((z_diff < lineSegPara.segGapTh_z) && (y_diff < lineSegPara.segGapTh_y))
{
a_run.len = i - a_run.start + 1;
a_run.value = i;
}
else
{
a_run.value = runIdx;
segs.push_back(a_run);
runIdx++;
a_run.start = i;
a_run.len = 1;
a_run.value = i;
}
}
pre_z = lineData[i].pt3D.z;
pre_y = lineData[i].pt3D.y;
}
}
if (a_run.len > 0)
segs.push_back(a_run);
//逐段处理
int segSize = (int)segs.size();
for (int si = 0; si < segSize; si++)
{
std::vector< SSG_RUN> segmentationLines;
split(segs[si], lineData, lineSegPara.maxDist, segmentationLines);
//转成SSG_featureSemiCircle格式
for (int m = 0, m_max = (int)segmentationLines.size(); m < m_max; m++)
{
SSG_featureSemiCircle a_seg;
a_seg.lineIdx = lineIdx;
a_seg.startPtIdx = segmentationLines[m].start;
a_seg.endPtIdx = segmentationLines[m].start + segmentationLines[m].len - 1;
SVzNL3DPoint ptStart = lineData[a_seg.startPtIdx].pt3D;
SVzNL3DPoint ptEnd = lineData[a_seg.endPtIdx].pt3D;
double len = sqrt(pow(ptStart.x - ptEnd.x, 2) + pow(ptStart.y - ptEnd.y, 2) + pow(ptStart.z - ptEnd.z, 2));
if ((len >= lineLenRange.min) && (len <= lineLenRange.max))
{
int midPtIdx = (a_seg.startPtIdx + a_seg.endPtIdx) / 2;
bool validMid = true;
if (lineData[midPtIdx].pt3D.z < 1e-4)
{
validMid = false;
int chkWin = 1;
while (1)
{
int idx = midPtIdx + chkWin;
if (idx >= a_seg.endPtIdx)
break;
if (lineData[idx].pt3D.z > 1e-4)
{
midPtIdx = idx;
validMid = true;
break;
}
idx = midPtIdx - chkWin;
if (idx <= a_seg.startPtIdx)
break;
if (lineData[idx].pt3D.z > 1e-4)
{
midPtIdx = idx;
validMid = true;
break;
}
chkWin++;
}
}
if (true == validMid)
{
a_seg.midPtIdx = midPtIdx;
a_seg.midPt = lineData[midPtIdx].pt3D;
lineSegs.push_back(a_seg);
}
}
}
}
}
bool compareByIdx(const SSG_pntDirAngle& a, const SSG_pntDirAngle& b) {
return a.pntIdx < b.pntIdx;
}

326
sourceCode/gasFillingPortPosition.cpp Normal file
View File

@ -0,0 +1,326 @@
#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "gasFillingPortPosition_Export.h"
#include <opencv2/opencv.hpp>
#include <limits>
//读版本号
//version 1.0.0 : base version release to customer
std::string m_strVersion = "1.0.0";
const char* wd_gasFillingPortPositionVersion(void)
{
return m_strVersion.c_str();
}
//提取加气口中心位姿
SSG_6DOF wd_getGasFillingPortPosition(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSX_gasFillingPortPara gasFillingPortPara,
const SSG_lineSegParam lineSegPara,
const SSG_outlierFilterParam filterParam,
SSG_treeGrowParam growParam,
int* errCode)
{
*errCode = 0;
SSG_6DOF resultPose;
memset(&resultPose, 0, sizeof(SSG_6DOF));
int lineNum = (int)scanLines.size();
int linePtNum = (int)scanLines[0].size();
bool isGridData = true;
//提取直线段特征
//垂直跳变特征提取
SVzNLRangeD lineLenRange;
lineLenRange.max = 1.2 * (gasFillingPortPara.outerD - gasFillingPortPara.innerD);
lineLenRange.min = 0.5 * (gasFillingPortPara.outerD - gasFillingPortPara.innerD) / 2;
std::vector<std::vector<SSG_featureSemiCircle>> lineFeatures_v_raw;
for (int line = 0; line < lineNum; line++)
{
if (line == 390)
int kkk = 1;
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
if (linePtNum != (int)lineData.size())
isGridData = false;
//滤波，滤除异常点
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
std::vector<SSG_featureSemiCircle> line_features;
int dataSize = (int)lineData.size();
wd_simpleLineSegment(
lineData,
line,
lineLenRange,
lineSegPara,
line_features);
lineFeatures_v_raw.push_back(line_features);
}
if (false == isGridData)//数据不是网格格式
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return resultPose;
}
//生成水平扫描
std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
hLines_raw.resize(linePtNum);
for (int i = 0; i < linePtNum; i++)
hLines_raw[i].resize(lineNum);
for (int line = 0; line < lineNum; line++)
{
for (int j = 0; j < linePtNum; j++)
{
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
hLines_raw[j][line] = scanLines[line][j];
hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y;
hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x;
}
}
//水平arc特征提取
std::vector<std::vector<SSG_featureSemiCircle>> lineFeatures_h_raw;
int lineNum_h_raw = (int)hLines_raw.size();
for (int line = 0; line < lineNum_h_raw; line++)
{
if (line == 974)
int kkk = 1;
std::vector<SVzNL3DPosition>& lineData = hLines_raw[line];
//滤波，滤除异常点
int ptNum = (int)lineData.size();
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam);
std::vector<SSG_featureSemiCircle> line_features;
int dataSize = (int)lineData.size();
wd_simpleLineSegment(
lineData,
line,
lineLenRange,
lineSegPara,
line_features);
lineFeatures_h_raw.push_back(line_features);
}
//标注
std::vector<std::vector<SSG_featureClusteringInfo>> featureInfoMask;
std::vector<std::vector<SVzNL3DPoint>> feature3DInfo;
featureInfoMask.resize(lineNum);
feature3DInfo.resize(lineNum);
for (int i = 0; i < lineNum; i++)
{
featureInfoMask[i].resize(lineNum_h_raw);
feature3DInfo[i].resize(lineNum_h_raw);
}
//垂直标注
for (int line = 0; line < lineNum; line++)
{
if (line == 390)
int kkk = 1;
std::vector<SSG_featureSemiCircle>& a_lineJumpFeature = lineFeatures_v_raw[line];
for (int m = 0, m_max = (int)a_lineJumpFeature.size(); m < m_max; m++)
{
int px = a_lineJumpFeature[m].lineIdx;
int py = a_lineJumpFeature[m].midPtIdx;
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[px][py];
a_featureInfo.clusterID = 0;
a_featureInfo.featurType = 1;
a_featureInfo.featureIdx_v = m;
a_featureInfo.featureIdx_h = 0;
a_featureInfo.lineIdx = px;
a_featureInfo.ptIdx = py;
a_featureInfo.flag = 0;
SVzNL3DPoint& a_feature3D = feature3DInfo[px][py];
a_feature3D = a_lineJumpFeature[m].midPt;
}
}
//水平标注
for (int line = 0; line < lineNum_h_raw; line++)
{
std::vector<SSG_featureSemiCircle>& a_lineJumpFeature = lineFeatures_h_raw[line];
for (int m = 0, m_max = (int)a_lineJumpFeature.size(); m < m_max; m++)
{
int py = a_lineJumpFeature[m].lineIdx;
int px = a_lineJumpFeature[m].midPtIdx;
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[px][py];
if (a_featureInfo.featurType == 0)
{
a_featureInfo.clusterID = 0;
a_featureInfo.lineIdx = px;
a_featureInfo.ptIdx = py;
a_featureInfo.flag = 0;
}
a_featureInfo.featurType += 2;
a_featureInfo.featureIdx_h = m;
SVzNL3DPoint& a_feature3DValue = feature3DInfo[px][py];
a_feature3DValue = { a_lineJumpFeature[m].midPt.y, a_lineJumpFeature[m].midPt.x, a_lineJumpFeature[m].midPt.z };
}
}
//聚类
//采用迭代思想，回归思路进行高效聚类
std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
std::vector<SVzNL3DRangeD> clustersRoi3D;
int clusterID = 1;
int clusterCheckWin = 5;
for (int y = 0; y < lineNum_h_raw; y++)
{
for (int x = 0; x < lineNum; x++)
{
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y];
if ( (0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //非特征或已经处理
continue;
SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y];
SVzNL3DRangeD a_clusterRoi;
a_clusterRoi.xRange.min = a_feature3DValue.x;
a_clusterRoi.xRange.max = a_feature3DValue.x;
a_clusterRoi.yRange.min = a_feature3DValue.y;
a_clusterRoi.yRange.max = a_feature3DValue.y;
a_clusterRoi.zRange.min = a_feature3DValue.z;
a_clusterRoi.zRange.max = a_feature3DValue.z;
SVzNL2DPoint a_seedPos = {x, y};
std::vector< SVzNL2DPoint> a_cluster;
a_cluster.push_back(a_seedPos);
wd_pointClustering2D(
featureInfoMask,//int，记录特征标记和clusterID，附加一个flag
feature3DInfo,//double,记录坐标信息
clusterCheckWin, //搜索窗口
growParam,//聚类条件
clusterID, //当前Cluster的ID
a_cluster, //result
a_clusterRoi
);
clusters.push_back(a_cluster);
clustersRoi3D.push_back(a_clusterRoi);
clusterID++;
}
}
//聚类结果分析
//取最前面的一个聚类
int clusterSize = (int)clusters.size();
int bestClusterIdx = -1;
double minZ = -1;
double wTh1 = gasFillingPortPara.innerD * 0.8;
double wTh2 = gasFillingPortPara.outerD * 1.1;
for (int i = 0; i < clusterSize; i++)
{
SVzNL3DRangeD& a_roi = clustersRoi3D[i];
double xWidth = a_roi.xRange.max - a_roi.xRange.min;
double yWidth = a_roi.yRange.max - a_roi.yRange.min;
double meanZ = (a_roi.zRange.min + a_roi.zRange.max) / 2;
if ((xWidth > wTh1) && (xWidth < wTh2) && (yWidth > wTh1) && (yWidth < wTh2))
{
if (minZ < 0)
{
minZ = meanZ;
bestClusterIdx = i;
}
else
{
if (minZ > meanZ)
{
minZ = meanZ;
bestClusterIdx = i;
}
}
}
}
if (minZ < 0)
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return resultPose;
}
std::vector< SVzNL2DPoint>& obj_cluster = clusters[bestClusterIdx];
//提取端面上的点
std::vector< cv::Point3f> planePts;
for (int i = 0, i_max = (int)obj_cluster.size(); i < i_max; i++)
{
SVzNL2DPoint a_pos = obj_cluster[i];
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[a_pos.x][a_pos.y];
int type_v = a_featureInfo.featurType & 0x01;
int type_h = a_featureInfo.featurType & 0x02;
if (type_v > 0)
{
int lineIdx = a_featureInfo.lineIdx;
int featureIdx = a_featureInfo.featureIdx_v;
SSG_featureSemiCircle& a_feature = lineFeatures_v_raw[lineIdx][featureIdx];
for (int m = a_feature.startPtIdx; m <= a_feature.endPtIdx; m++)
{
SVzNL3DPosition& a_pt3d = scanLines[lineIdx][m];
if ((a_pt3d.nPointIdx == 0) && (a_pt3d.pt3D.z > 1e-4))
{
a_pt3d.nPointIdx = 1;
cv::Point3f a_planePt = { (float)a_pt3d.pt3D.x, (float)a_pt3d.pt3D.y, (float)a_pt3d.pt3D.z };
planePts.push_back(a_planePt);
}
}
}
if (type_h > 0)
{
int ptIdx = a_featureInfo.ptIdx;
int featureIdx = a_featureInfo.featureIdx_h;
SSG_featureSemiCircle& a_feature = lineFeatures_h_raw[ptIdx][featureIdx];
for (int m = a_feature.startPtIdx; m <= a_feature.endPtIdx; m++)
{
SVzNL3DPosition& a_pt3d = scanLines[m][ptIdx];
if ( (a_pt3d.nPointIdx == 0) && (a_pt3d.pt3D.z >1e-4))
{
a_pt3d.nPointIdx = 1;
cv::Point3f a_planePt = { (float)a_pt3d.pt3D.x, (float)a_pt3d.pt3D.y, (float)a_pt3d.pt3D.z };
planePts.push_back(a_planePt);
}
}
}
}
// 拟合平面，计算法向
//计算面参数: z = Ax + By + C
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
std::vector<double> res;
vzCaculateLaserPlane(planePts, res);
//将平面调整为水平平面
SSG_planeCalibPara calibPara = adjustPlaneToXYPlane(
res[0], res[1], res[2] //平面法向量
);
//投影
std::vector< SVzNL3DPoint> projectionPts;
double calibMeanZ = 0;
int planePtSize = (int)planePts.size();
for(int i = 0; i < planePtSize; i ++)
{
SVzNL3DPoint a_calibPt;
a_calibPt.x = (float)(planePts[i].x * calibPara.planeCalib[0] + planePts[i].y * calibPara.planeCalib[1] + planePts[i].z * calibPara.planeCalib[2]);
a_calibPt.y = (float)(planePts[i].x * calibPara.planeCalib[3] + planePts[i].y * calibPara.planeCalib[4] + planePts[i].z * calibPara.planeCalib[5]);
a_calibPt.z = (float)(planePts[i].x * calibPara.planeCalib[6] + planePts[i].y * calibPara.planeCalib[7] + planePts[i].z * calibPara.planeCalib[8]);
calibMeanZ += a_calibPt.z;
projectionPts.push_back(a_calibPt);
}
calibMeanZ = calibMeanZ / planePtSize;
//圆最小二乘拟合
SVzNL3DPoint calibCenter;
double radius = 0;
double fittingErr = fitCircleByLeastSquare(
projectionPts,
calibCenter,
radius);
calibCenter.z = calibMeanZ;
//center点旋转回去
SVzNL3DPoint center;
center.x = (float)(calibCenter.x * calibPara.invRMatrix[0] + calibCenter.y * calibPara.invRMatrix[1] + calibCenter.z * calibPara.invRMatrix[2]);
center.y = (float)(calibCenter.x * calibPara.invRMatrix[3] + calibCenter.y * calibPara.invRMatrix[4] + calibCenter.z * calibPara.invRMatrix[5]);
center.z = (float)(calibCenter.x * calibPara.invRMatrix[6] + calibCenter.y * calibPara.invRMatrix[7] + calibCenter.z * calibPara.invRMatrix[8]);
resultPose.x = center.x;
resultPose.y = center.y;
resultPose.z = center.z;
resultPose.x_roll = res[0];
resultPose.y_pitch = res[1];
resultPose.z_yaw = res[2];
return resultPose;
}

25
sourceCode/gasFillingPortPosition_Export.h Normal file
View File

@ -0,0 +1,25 @@
#pragma once
#include "SG_algo_Export.h"
#include <vector>
#define _OUTPUT_DEBUG_DATA 1
typedef struct
{
double innerD; //加气口端面内径，42mm
double outerD; //加气口端面外径，52mm
}SSX_gasFillingPortPara;
//读版本号
SG_APISHARED_EXPORT const char* wd_gasFillingPortPositionVersion(void);
//提取加气口中心位姿
SG_APISHARED_EXPORT SSG_6DOF wd_getGasFillingPortPosition(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSX_gasFillingPortPara gasFillingPortPara,
const SSG_lineSegParam lineSegPara,
const SSG_outlierFilterParam filterParam,
SSG_treeGrowParam growParam,
int* errCode);