Compare commits
No commits in common. "497b98e2c35f4235b1b4b3255de2be0d3632e304" and "1026adaa5c5562113a9ecf5c4adb3e97c2576316" have entirely different histories.
497b98e2c3
...
1026adaa5c
@ -1,172 +0,0 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<ItemGroup Label="ProjectConfigurations">
|
||||
<ProjectConfiguration Include="Debug|Win32">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|Win32">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Debug|x64">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|x64">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="..\sourceCode\WD_QRcode3Ddetection.cpp" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClInclude Include="..\sourceCode\WD_QRcode3Ddetection_Export.h" />
|
||||
</ItemGroup>
|
||||
<PropertyGroup Label="Globals">
|
||||
<VCProjectVersion>16.0</VCProjectVersion>
|
||||
<Keyword>Win32Proj</Keyword>
|
||||
<ProjectGuid>{a7c72ed9-b81d-4c6a-be38-d75199c824aa}</ProjectGuid>
|
||||
<RootNamespace>QRcode3Ddetection</RootNamespace>
|
||||
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
|
||||
<ImportGroup Label="ExtensionSettings">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="Shared">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<PropertyGroup Label="UserMacros" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
</PropertyGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>WIN32;_DEBUG;QRCODE3DDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<PrecompiledHeader>Use</PrecompiledHeader>
|
||||
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Windows</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<EnableUAC>false</EnableUAC>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>WIN32;NDEBUG;QRCODE3DDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<PrecompiledHeader>Use</PrecompiledHeader>
|
||||
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Windows</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<EnableUAC>false</EnableUAC>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>SG_API_LIBRARY;_DEBUG;QRCODE3DDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||
<AdditionalIncludeDirectories>..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Windows</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<EnableUAC>false</EnableUAC>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
<AdditionalDependencies>opencv_world480d.lib;baseAlgorithm.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>SG_API_LIBRARY;NDEBUG;QRCODE3DDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||
<AdditionalIncludeDirectories>..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Windows</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<EnableUAC>false</EnableUAC>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
<AdditionalDependencies>opencv_world480.lib;baseAlgorithm.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||
<ImportGroup Label="ExtensionTargets">
|
||||
</ImportGroup>
|
||||
</Project>
|
||||
@ -1,745 +0,0 @@
|
||||
// QRcode3Ddetection_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
|
||||
//
|
||||
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <vector>
|
||||
#include <stdio.h>
|
||||
#include <VZNL_Types.h>
|
||||
#include "direct.h"
|
||||
#include <string>
|
||||
#include "WD_QRcode3Ddetection_Export.h"
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <Windows.h>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int r;
|
||||
int g;
|
||||
int b;
|
||||
}SG_color;
|
||||
|
||||
void vzReadLaserScanPointFromFile_plyTxt(const char* fileName, std::vector< SVzNL3DPoint>& scanData, bool removeZeros, bool exchangeXY)
|
||||
{
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
|
||||
if (inputFile.is_open() == false)
|
||||
return;
|
||||
|
||||
while (std::getline(inputFile, linedata))
|
||||
{
|
||||
if (linedata.empty())
|
||||
continue;
|
||||
|
||||
double X, Y, Z, tmp;
|
||||
sscanf_s(linedata.c_str(), "%lf %lf %lf %lf", &X, &Y, &Z, &tmp);
|
||||
if (true == removeZeros)
|
||||
{
|
||||
if (Z > 1e-4)
|
||||
{
|
||||
SVzNL3DPoint a_pt;
|
||||
if (true == exchangeXY)
|
||||
{
|
||||
a_pt.x = Y; //将扫描线调整为Y方向；扫描方向为X方向
|
||||
a_pt.y = X;
|
||||
}
|
||||
else
|
||||
{
|
||||
a_pt.x = X;
|
||||
a_pt.y = Y;
|
||||
}
|
||||
a_pt.z = Z;
|
||||
scanData.push_back(a_pt);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
SVzNL3DPoint a_pt;
|
||||
if (true == exchangeXY)
|
||||
{
|
||||
a_pt.x = Y; //将扫描线调整为Y方向；扫描方向为X方向
|
||||
a_pt.y = X;
|
||||
}
|
||||
else
|
||||
{
|
||||
a_pt.x = X;
|
||||
a_pt.y = Y;
|
||||
}
|
||||
a_pt.z = Z;
|
||||
scanData.push_back(a_pt);
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void _outputScanDataFile_vector(char* fileName, std::vector<std::vector<SVzNL3DPosition>>& scanLines, bool removeZeros, int* headNullLines)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
int lineNum = scanLines.size();
|
||||
if (lineNum == 0)
|
||||
return;
|
||||
|
||||
sw << "LineNum:" << lineNum << std::endl;
|
||||
sw << "DataType: 0" << std::endl;
|
||||
sw << "ScanSpeed: 0" << std::endl;
|
||||
sw << "PointAdjust: 1" << std::endl;
|
||||
sw << "MaxTimeStamp: 0_0" << std::endl;
|
||||
|
||||
int lineIdx = 0;
|
||||
int null_lines = 0;
|
||||
bool counterNull = true;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int linePtNum = scanLines[line].size();
|
||||
if (linePtNum == 0)
|
||||
continue;
|
||||
|
||||
if (true == removeZeros)
|
||||
{
|
||||
int vldPtNum = 0;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
if (scanLines[line][i].pt3D.z > 1e-4)
|
||||
vldPtNum++;
|
||||
}
|
||||
linePtNum = vldPtNum;
|
||||
}
|
||||
sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
|
||||
lineIdx++;
|
||||
bool isNull = true;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
SVzNL3DPoint* pt3D = &scanLines[line][i].pt3D;
|
||||
if ((pt3D->z > 1e-4) && (isNull == true))
|
||||
isNull = false;
|
||||
if ((true == removeZeros) && (pt3D->z < 1e-4))
|
||||
continue;
|
||||
float x = (float)pt3D->x;
|
||||
float y = (float)pt3D->y;
|
||||
float z = (float)pt3D->z;
|
||||
sw << "{ " << x << "," << y << "," << z << " }-";
|
||||
sw << "{0,0}-{0,0}" << std::endl;
|
||||
}
|
||||
if (true == counterNull)
|
||||
{
|
||||
if (true == isNull)
|
||||
null_lines++;
|
||||
else
|
||||
counterNull = false;
|
||||
}
|
||||
}
|
||||
*headNullLines = null_lines;
|
||||
sw.close();
|
||||
}
|
||||
|
||||
void _outputRGBDScanDataFile_RGBD(
|
||||
char* fileName,
|
||||
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
|
||||
std::vector< SVzNL3DPosition> objPoints)
|
||||
{
|
||||
int lineNum = scanLines.size();
|
||||
std::ofstream sw(fileName);
|
||||
int realLines = lineNum;
|
||||
if (objPoints.size() > 0)
|
||||
realLines++;
|
||||
|
||||
sw << "LineNum:" << realLines << std::endl;
|
||||
sw << "DataType: 0" << std::endl;
|
||||
sw << "ScanSpeed: 0" << std::endl;
|
||||
sw << "PointAdjust: 1" << std::endl;
|
||||
sw << "MaxTimeStamp: 0_0" << std::endl;
|
||||
|
||||
int maxLineIndex = 0;
|
||||
int max_stamp = 0;
|
||||
|
||||
SG_color rgb = { 0, 0, 0 };
|
||||
|
||||
SG_color objColor[8] = {
|
||||
{245,222,179},//淡黄色
|
||||
{210,105, 30},//巧克力色
|
||||
{240,230,140},//黄褐色
|
||||
{135,206,235},//天蓝色
|
||||
{250,235,215},//古董白
|
||||
{189,252,201},//薄荷色
|
||||
{221,160,221},//梅红色
|
||||
{188,143,143},//玫瑰红色
|
||||
};
|
||||
int size = 1;
|
||||
int lineIdx = 0;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int linePtNum = scanLines[line].size();
|
||||
if (linePtNum == 0)
|
||||
continue;
|
||||
|
||||
sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
|
||||
lineIdx++;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
SVzNL3DPosition* pt3D = &scanLines[line][i];
|
||||
int featureType = pt3D->nPointIdx;
|
||||
if (LINE_FEATURE_PEAK_TOP == featureType)
|
||||
{
|
||||
rgb = { 255, 97, 0 };
|
||||
size = 5;
|
||||
}
|
||||
else
|
||||
{
|
||||
rgb = { 200, 200, 200 };
|
||||
size = 1;
|
||||
}
|
||||
float x = (float)pt3D->pt3D.x;
|
||||
float y = (float)pt3D->pt3D.y;
|
||||
float z = (float)pt3D->pt3D.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
}
|
||||
if (objPoints.size() > 0)
|
||||
{
|
||||
int linePtNum = objPoints.size();
|
||||
sw << "Line_" << lineNum << "_0_" << linePtNum+1 << std::endl;
|
||||
|
||||
rgb = { 0, 0, 255 };
|
||||
size = 25;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
float x = (float)objPoints[i].pt3D.x;
|
||||
float y = (float)objPoints[i].pt3D.y;
|
||||
float z = (float)objPoints[i].pt3D.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
float x = (float)objPoints[0].pt3D.x;
|
||||
float y = (float)objPoints[0].pt3D.y;
|
||||
float z = (float)objPoints[0].pt3D.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
sw.close();
|
||||
}
|
||||
|
||||
|
||||
void _outputScanDataFile_ptr(char* fileName, SVzNL3DLaserLine* scanData, int lineNum)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
sw << "LineNum:" << lineNum << std::endl;
|
||||
sw << "DataType: 0" << std::endl;
|
||||
sw << "ScanSpeed: 0" << std::endl;
|
||||
sw << "PointAdjust: 1" << std::endl;
|
||||
sw << "MaxTimeStamp: 0_0" << std::endl;
|
||||
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
sw << "Line_" << line << "_" << scanData[line].nTimeStamp << "_" << scanData[line].nPositionCnt << std::endl;
|
||||
for (int i = 0; i < scanData[line].nPositionCnt; i++)
|
||||
{
|
||||
SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i];
|
||||
float x = (float)pt3D->pt3D.x;
|
||||
float y = (float)pt3D->pt3D.y;
|
||||
float z = (float)pt3D->pt3D.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}" << std::endl;
|
||||
}
|
||||
}
|
||||
sw.close();
|
||||
}
|
||||
|
||||
|
||||
#define DATA_VER_OLD 0
|
||||
#define DATA_VER_NEW 1
|
||||
#define DATA_VER_FROM_CUSTOM 2
|
||||
#define VZ_LASER_LINE_PT_MAX_NUM 4096
|
||||
SVzNL3DLaserLine* vzReadLaserScanPointFromFile_XYZ(const char* fileName, int* scanLineNum, float* scanV,
|
||||
int* dataCalib, int* scanMaxStamp, int* canClockUnit)
|
||||
{
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
|
||||
if (inputFile.is_open() == false)
|
||||
return NULL;
|
||||
|
||||
SVzNL3DLaserLine* _scanLines = NULL;
|
||||
|
||||
int lines = 0;
|
||||
int dataElements = 4;
|
||||
int firstIndex = -1;
|
||||
|
||||
int dataFileVer = DATA_VER_OLD;
|
||||
std::getline(inputFile, linedata); //第一行
|
||||
int lineNum = 0;
|
||||
if (0 == strncmp("LineNum:", linedata.c_str(), 8))
|
||||
{
|
||||
dataFileVer = DATA_VER_NEW;
|
||||
sscanf_s(linedata.c_str(), "LineNum:%d", &lines);
|
||||
if (lines == 0)
|
||||
return NULL;
|
||||
lineNum = lines;
|
||||
_scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1));
|
||||
memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1));
|
||||
if (scanLineNum)
|
||||
*scanLineNum = lines;
|
||||
}
|
||||
else if (0 == strncmp("LineNum_", linedata.c_str(), 8))
|
||||
{
|
||||
dataFileVer = DATA_VER_OLD;
|
||||
sscanf_s(linedata.c_str(), "LineNum_%d", &lines);
|
||||
if (lines == 0)
|
||||
return NULL;
|
||||
lineNum = lines;
|
||||
_scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1));
|
||||
memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1));
|
||||
if (scanLineNum)
|
||||
*scanLineNum = lines;
|
||||
}
|
||||
if (_scanLines == NULL)
|
||||
return NULL;
|
||||
|
||||
int ptNum = 0;
|
||||
int lineIdx = -1;
|
||||
int ptIdx = 0;
|
||||
SVzNL3DPosition* p3DPoint = NULL;
|
||||
if (dataFileVer == DATA_VER_NEW)
|
||||
{
|
||||
while (getline(inputFile, linedata))
|
||||
{
|
||||
if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10))
|
||||
{
|
||||
double lineV = 0;
|
||||
sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV);
|
||||
if (scanV)
|
||||
*scanV = (float)lineV;
|
||||
}
|
||||
else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12))
|
||||
{
|
||||
int ptAdjusted = 0;
|
||||
sscanf_s(linedata.c_str(), "PointAdjust:%d", &ptAdjusted);
|
||||
if (dataCalib)
|
||||
*dataCalib = ptAdjusted;
|
||||
}
|
||||
else if (0 == strncmp("MaxTimeStamp:", linedata.c_str(), 13))
|
||||
{
|
||||
unsigned int maxTimeStamp = 0;
|
||||
unsigned int timePerStamp = 0;
|
||||
sscanf_s(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp);
|
||||
if (scanMaxStamp)
|
||||
*scanMaxStamp = maxTimeStamp;
|
||||
if (canClockUnit)
|
||||
*canClockUnit = timePerStamp;
|
||||
}
|
||||
else if (0 == strncmp("Line_", linedata.c_str(), 5))
|
||||
{
|
||||
int lineIndex;
|
||||
unsigned int timeStamp;
|
||||
sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum);
|
||||
if (firstIndex < 0)
|
||||
firstIndex = lineIndex;
|
||||
|
||||
lineIndex = lineIndex - firstIndex;
|
||||
if ((lineIndex < 0) || (lineIndex >= lines))
|
||||
break;
|
||||
|
||||
//new Line
|
||||
lineIdx++;
|
||||
if (ptNum > 0)
|
||||
{
|
||||
p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * ptNum);
|
||||
memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum);
|
||||
}
|
||||
else
|
||||
p3DPoint = NULL;
|
||||
_scanLines[lineIdx].nPositionCnt = 0;
|
||||
_scanLines[lineIdx].nTimeStamp = timeStamp;
|
||||
_scanLines[lineIdx].p3DPosition = p3DPoint;
|
||||
|
||||
}
|
||||
else if (0 == strncmp("{", linedata.c_str(), 1))
|
||||
{
|
||||
float X, Y, Z;
|
||||
int imageY = 0;
|
||||
float leftX, leftY;
|
||||
float rightX, rightY;
|
||||
sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
|
||||
int id = _scanLines[lineIdx].nPositionCnt;
|
||||
if (id < ptNum)
|
||||
{
|
||||
p3DPoint[id].pt3D.x = X;
|
||||
p3DPoint[id].pt3D.y = Y;
|
||||
p3DPoint[id].pt3D.z = Z;
|
||||
_scanLines[lineIdx].nPositionCnt = id + 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
else if (dataFileVer == DATA_VER_OLD)
|
||||
{
|
||||
while (getline(inputFile, linedata))
|
||||
{
|
||||
if (0 == strncmp("DataElements_", linedata.c_str(), 13))
|
||||
{
|
||||
sscanf_s(linedata.c_str(), "DataElements_%d", &dataElements);
|
||||
if ((dataElements != 3) && (dataElements != 4))
|
||||
break;
|
||||
}
|
||||
if (0 == strncmp("LineV_", linedata.c_str(), 6))
|
||||
{
|
||||
double lineV = 0;
|
||||
sscanf_s(linedata.c_str(), "LineV_%lf", &lineV);
|
||||
}
|
||||
else if (0 == strncmp("Line_", linedata.c_str(), 5))
|
||||
{
|
||||
int lineIndex;
|
||||
unsigned int timeStamp;
|
||||
sscanf_s(linedata.c_str(), "Line_%d_%u", &lineIndex, &timeStamp);
|
||||
#if 0
|
||||
if (scanLineListTail == NULL)
|
||||
firstIndex = lineIndex;
|
||||
#endif
|
||||
lineIndex = lineIndex - firstIndex;
|
||||
if ((lineIndex < 0) || (lineIndex >= lines))
|
||||
break;
|
||||
//new Line
|
||||
//new Line
|
||||
lineIdx++;
|
||||
p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM);
|
||||
memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM);
|
||||
_scanLines[lineIdx].nPositionCnt = 0;
|
||||
_scanLines[lineIdx].nTimeStamp = timeStamp;
|
||||
_scanLines[lineIdx].p3DPosition = p3DPoint;
|
||||
}
|
||||
else if (0 == strncmp("(", linedata.c_str(), 1))
|
||||
{
|
||||
float X, Y, Z;
|
||||
int imageY = 0;
|
||||
if (dataElements == 4)
|
||||
sscanf_s(linedata.c_str(), "(%f,%f,%f,%d)", &X, &Y, &Z, &imageY);
|
||||
else
|
||||
sscanf_s(linedata.c_str(), "(%f,%f,%f)", &X, &Y, &Z);
|
||||
int id = _scanLines[lineIdx].nPositionCnt;
|
||||
if (id < VZ_LASER_LINE_PT_MAX_NUM)
|
||||
{
|
||||
p3DPoint[id].pt3D.x = X;
|
||||
p3DPoint[id].pt3D.y = Y;
|
||||
p3DPoint[id].pt3D.z = Z;
|
||||
_scanLines[lineIdx].nPositionCnt = id + 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
inputFile.close();
|
||||
return _scanLines;
|
||||
}
|
||||
|
||||
void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector< SVzNL3DPoint>& scanData)
|
||||
{
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
|
||||
if (inputFile.is_open() == false)
|
||||
return;
|
||||
|
||||
while (getline(inputFile, linedata))
|
||||
{
|
||||
if (0 == strncmp("{", linedata.c_str(), 1))
|
||||
{
|
||||
float X, Y, Z;
|
||||
int imageY = 0;
|
||||
float leftX, leftY;
|
||||
float rightX, rightY;
|
||||
sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
|
||||
SVzNL3DPoint a_pt;
|
||||
a_pt.x = X;
|
||||
a_pt.y = Y;
|
||||
a_pt.z = Z;
|
||||
scanData.push_back(a_pt);
|
||||
}
|
||||
}
|
||||
inputFile.close();
|
||||
return;
|
||||
}
|
||||
|
||||
void _outputCalibPara(char* fileName, SSG_planeCalibPara calibPara)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
char dataStr[250];
|
||||
//调平矩阵
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[0], calibPara.planeCalib[1], calibPara.planeCalib[2]);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[3], calibPara.planeCalib[4], calibPara.planeCalib[5]);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[6], calibPara.planeCalib[7], calibPara.planeCalib[8]);
|
||||
sw << dataStr << std::endl;
|
||||
//地面高度
|
||||
sprintf_s(dataStr, 250, "%g", calibPara.planeHeight);
|
||||
sw << dataStr << std::endl;
|
||||
//反向旋转矩阵
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[0], calibPara.invRMatrix[1], calibPara.invRMatrix[2]);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[3], calibPara.invRMatrix[4], calibPara.invRMatrix[5]);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[6], calibPara.invRMatrix[7], calibPara.invRMatrix[8]);
|
||||
sw << dataStr << std::endl;
|
||||
|
||||
sw.close();
|
||||
}
|
||||
|
||||
SSG_planeCalibPara _readCalibPara(char* fileName)
|
||||
{
|
||||
//设置初始结果
|
||||
double initCalib[9] = {
|
||||
1.0, 0.0, 0.0,
|
||||
0.0, 1.0, 0.0,
|
||||
0.0, 0.0, 1.0 };
|
||||
SSG_planeCalibPara planePara;
|
||||
for (int i = 0; i < 9; i++)
|
||||
planePara.planeCalib[i] = initCalib[i];
|
||||
planePara.planeHeight = -1.0;
|
||||
for (int i = 0; i < 9; i++)
|
||||
planePara.invRMatrix[i] = initCalib[i];
|
||||
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
|
||||
if (inputFile.is_open() == false)
|
||||
return planePara;
|
||||
|
||||
//调平矩阵
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[0], &planePara.planeCalib[1], &planePara.planeCalib[2]);
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[3], &planePara.planeCalib[4], &planePara.planeCalib[5]);
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[6], &planePara.planeCalib[7], &planePara.planeCalib[8]);
|
||||
//地面高度
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf", &planePara.planeHeight);
|
||||
//反向旋转矩阵
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[0], &planePara.invRMatrix[1], &planePara.invRMatrix[2]);
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[3], &planePara.invRMatrix[4], &planePara.invRMatrix[5]);
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[6], &planePara.invRMatrix[7], &planePara.invRMatrix[8]);
|
||||
|
||||
inputFile.close();
|
||||
return planePara;
|
||||
}
|
||||
|
||||
void _getRoiClouds(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
int startLine,
|
||||
int endLine,
|
||||
int startPtIdx,
|
||||
int endPtIdx,
|
||||
std::vector< std::vector<SVzNL3DPosition>>& roiScanLines)
|
||||
{
|
||||
for (int i = startLine; i < endLine; i++)
|
||||
{
|
||||
if (i >= scanLines.size())
|
||||
break;
|
||||
|
||||
std::vector<SVzNL3DPosition> cut_line;
|
||||
std::vector<SVzNL3DPosition>& a_line = scanLines[i];
|
||||
for (int j = startPtIdx; j < endPtIdx; j++)
|
||||
{
|
||||
SVzNL3DPosition a_pt;
|
||||
if (j >= a_line.size())
|
||||
{
|
||||
a_pt.nPointIdx = 0;
|
||||
a_pt.pt3D = { 0,0,0 };
|
||||
}
|
||||
else
|
||||
a_pt = a_line[j];
|
||||
cut_line.push_back(a_pt);
|
||||
}
|
||||
roiScanLines.push_back(cut_line);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
#define TEST_COMPUTE_CALIB_PARA 0
|
||||
#define TEST_COMPUTE_QRCODE_IMG 1
|
||||
#define TEST_GROUP 1
|
||||
int main()
|
||||
{
|
||||
const char* dataPath[TEST_GROUP] = {
|
||||
"F:\\ShangGu\\项目\\工件端部圆点二维码\\", //0
|
||||
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
{3,3}
|
||||
};
|
||||
|
||||
#if TEST_COMPUTE_CALIB_PARA
|
||||
char _calib_datafile[256];
|
||||
sprintf_s(_calib_datafile, "F:\\ShangGu\\项目\\工件端部圆点二维码\\LaserLine3_grid.txt");
|
||||
int lineNum = 0;
|
||||
float lineV = 0.0f;
|
||||
int dataCalib = 0;
|
||||
int maxTimeStamp = 0;
|
||||
int clockPerSecond = 0;
|
||||
SVzNL3DLaserLine* laser3DPoints = vzReadLaserScanPointFromFile_XYZ(_calib_datafile, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond);
|
||||
if (laser3DPoints)
|
||||
{
|
||||
SSG_planeCalibPara calibPara = wd_getBaseCalibPara(
|
||||
laser3DPoints,
|
||||
lineNum);
|
||||
//结果进行验证
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
if (i == 14)
|
||||
int kkk = 1;
|
||||
//行处理
|
||||
//调平，去除地面
|
||||
wd_lineDataR(&laser3DPoints[i], calibPara.planeCalib, -1);// calibPara.planeHeight);
|
||||
}
|
||||
//
|
||||
char calibFile[250];
|
||||
sprintf_s(calibFile, "F:\\ShangGu\\项目\\工件端部圆点二维码\\ground_calib_para.txt");
|
||||
_outputCalibPara(calibFile, calibPara);
|
||||
char _out_file[256];
|
||||
sprintf_s(_out_file, "F:\\ShangGu\\项目\\工件端部圆点二维码\\LaserLine3_calib_data.txt");
|
||||
_outputScanDataFile_ptr(_out_file, laser3DPoints, lineNum);
|
||||
printf("%s: calib done!\n", _calib_datafile);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#if TEST_COMPUTE_QRCODE_IMG
|
||||
for (int grp = 0; grp <= 0; grp++)
|
||||
{
|
||||
int cloud_rows = 4200;
|
||||
int cloud_cols = 2160;
|
||||
SSG_planeCalibPara poseCalibPara;
|
||||
//初始化成单位阵
|
||||
poseCalibPara.planeCalib[0] = 1.0;
|
||||
poseCalibPara.planeCalib[1] = 0.0;
|
||||
poseCalibPara.planeCalib[2] = 0.0;
|
||||
poseCalibPara.planeCalib[3] = 0.0;
|
||||
poseCalibPara.planeCalib[4] = 1.0;
|
||||
poseCalibPara.planeCalib[5] = 0.0;
|
||||
poseCalibPara.planeCalib[6] = 0.0;
|
||||
poseCalibPara.planeCalib[7] = 0.0;
|
||||
poseCalibPara.planeCalib[8] = 1.0;
|
||||
poseCalibPara.planeHeight = -1.0;
|
||||
for (int i = 0; i < 9; i++)
|
||||
poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i];
|
||||
char calibFile[250];
|
||||
sprintf_s(calibFile, "F:\\ShangGu\\项目\\工件端部圆点二维码\\ground_calib_para.txt");
|
||||
poseCalibPara = _readCalibPara(calibFile);
|
||||
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx = 1;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%s%d_Cloud.txt", dataPath[grp], fidx);
|
||||
std::vector< SVzNL3DPoint> scanData;
|
||||
bool removeZeros = false;
|
||||
bool exchangeXY = true;
|
||||
if (fidx < 3)
|
||||
{
|
||||
cloud_cols = 1; //单行测试文件
|
||||
exchangeXY = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
cloud_cols = 2160; //单行测试文件
|
||||
exchangeXY = true;
|
||||
}
|
||||
if (fidx == 2)
|
||||
{
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanData);
|
||||
cloud_cols = 1100;
|
||||
cloud_rows = 1300;
|
||||
if (scanData.size() != (cloud_rows * cloud_cols))
|
||||
continue;
|
||||
}
|
||||
else
|
||||
{
|
||||
vzReadLaserScanPointFromFile_plyTxt(_scan_file, scanData, removeZeros, exchangeXY);
|
||||
if (scanData.size() != (cloud_rows * cloud_cols))
|
||||
continue;
|
||||
}
|
||||
//将数据恢复为按扫描线存储格式
|
||||
std::vector< std::vector<SVzNL3DPosition>> scanLines;
|
||||
wd_getScanLines(scanData, scanLines, cloud_rows);
|
||||
|
||||
#if 0
|
||||
//点云裁剪
|
||||
std::vector< std::vector<SVzNL3DPosition>> roiScanLines;
|
||||
_getRoiClouds(scanLines, 700, 1800, 400, 1700, roiScanLines);
|
||||
sprintf_s(_scan_file, "%sLaserLine4_grid.txt", dataPath[grp]);
|
||||
int headNullLines = 0;
|
||||
_outputScanDataFile_vector(_scan_file, roiScanLines, false, &headNullLines);
|
||||
#endif
|
||||
#if 0
|
||||
sprintf_s(_scan_file, "%sLaserLine%d_grid.txt", dataPath[grp], fidx);
|
||||
int headNullLines = 0;
|
||||
_outputScanDataFile_vector(_scan_file, scanLines, false, &headNullLines);
|
||||
printf("%s: head null lines = %d\n", _scan_file, headNullLines);
|
||||
#endif
|
||||
WD_QRcodeParam qrcode_param;
|
||||
qrcode_param.rows = 14;
|
||||
qrcode_param.cols = 14;
|
||||
qrcode_param.row_space = 30.0 / 14;
|
||||
qrcode_param.col_space = 30.0 / 14;
|
||||
qrcode_param.pointHoleDepth = 1.0;
|
||||
qrcode_param.pointHoleR = 0.6;
|
||||
|
||||
long t1 = GetTickCount64();//统计时间
|
||||
|
||||
for (int i = 0, i_max= scanLines.size(); i < i_max; i++)
|
||||
{
|
||||
if (i == 14)
|
||||
int kkk = 1;
|
||||
//行处理
|
||||
//调平，去除地面
|
||||
wd_lineDataR(scanLines[i], poseCalibPara.planeCalib, -1);
|
||||
}
|
||||
//
|
||||
#if 0
|
||||
sprintf_s(_scan_file, "%sresult\\LaserLine%d_calib_data.txt", dataPath[grp], fidx);
|
||||
int headNulls = 0;
|
||||
_outputScanDataFile_vector(_scan_file, scanLines, true, &headNulls);
|
||||
#endif
|
||||
cv::Mat dmCodeImg;
|
||||
std::vector< SVzNL3DPosition> objPoints;
|
||||
wd_QRcode3Ddetection(
|
||||
scanLines,
|
||||
qrcode_param,
|
||||
objPoints,
|
||||
dmCodeImg);
|
||||
|
||||
long t2 = GetTickCount64();
|
||||
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
|
||||
//输出测试结果
|
||||
sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
|
||||
_outputRGBDScanDataFile_RGBD(_scan_file, scanLines, objPoints);
|
||||
if (!dmCodeImg.empty())
|
||||
{
|
||||
sprintf_s(_scan_file, "%sresult\\LaserLine%d_img.png", dataPath[grp], fidx);
|
||||
cv::Mat normDmCodeImg;
|
||||
cv::normalize(dmCodeImg, normDmCodeImg, 0, 255, cv::NORM_MINMAX, CV_8U);
|
||||
cv::imwrite(_scan_file, normDmCodeImg);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
|
||||
// 调试程序: F5 或调试 >“开始调试”菜单
|
||||
|
||||
// 入门使用技巧:
|
||||
// 1. 使用解决方案资源管理器窗口添加/管理文件
|
||||
// 2. 使用团队资源管理器窗口连接到源代码管理
|
||||
// 3. 使用输出窗口查看生成输出和其他消息
|
||||
// 4. 使用错误列表窗口查看错误
|
||||
// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
|
||||
// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件
|
||||
@ -1,157 +0,0 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<ItemGroup Label="ProjectConfigurations">
|
||||
<ProjectConfiguration Include="Debug|Win32">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|Win32">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Debug|x64">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|x64">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
</ItemGroup>
|
||||
<PropertyGroup Label="Globals">
|
||||
<VCProjectVersion>16.0</VCProjectVersion>
|
||||
<Keyword>Win32Proj</Keyword>
|
||||
<ProjectGuid>{e5faaacf-703c-4460-93b8-301991f62778}</ProjectGuid>
|
||||
<RootNamespace>QRcode3Ddetectiontest</RootNamespace>
|
||||
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
|
||||
<ImportGroup Label="ExtensionSettings">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="Shared">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<PropertyGroup Label="UserMacros" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||
</PropertyGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<AdditionalIncludeDirectories>..\..\thirdParty\opencv\build\include;</AdditionalIncludeDirectories>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
<AdditionalDependencies>opencv_world480d.lib;QRcode3Ddetection.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<AdditionalIncludeDirectories>..\..\thirdParty\opencv\build\include;</AdditionalIncludeDirectories>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
<AdditionalDependencies>opencv_world480.lib;QRcode3Ddetection.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="QRcode3Ddetection_test.cpp" />
|
||||
</ItemGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||
<ImportGroup Label="ExtensionTargets">
|
||||
</ImportGroup>
|
||||
</Project>
|
||||
@ -66,16 +66,6 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "SieveNodeDetection_test", "
|
||||
{D47DE411-B18C-4A64-BDB9-0AFAB50E8A8A} = {D47DE411-B18C-4A64-BDB9-0AFAB50E8A8A}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "QRcode3Ddetection", "QRcode3Ddetection\QRcode3Ddetection.vcxproj", "{A7C72ED9-B81D-4C6A-BE38-D75199C824AA}"
|
||||
ProjectSection(ProjectDependencies) = postProject
|
||||
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "QRcode3Ddetection_test", "QRcode3Ddetection_test\QRcode3Ddetection_test.vcxproj", "{E5FAAACF-703C-4460-93B8-301991F62778}"
|
||||
ProjectSection(ProjectDependencies) = postProject
|
||||
{A7C72ED9-B81D-4C6A-BE38-D75199C824AA} = {A7C72ED9-B81D-4C6A-BE38-D75199C824AA}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Global
|
||||
GlobalSection(SolutionConfigurationPlatforms) = preSolution
|
||||
Debug|x64 = Debug|x64
|
||||
@ -188,22 +178,6 @@ Global
|
||||
{DF0881C7-43D0-4765-97CA-74C126665F3E}.Release|x64.Build.0 = Release|x64
|
||||
{DF0881C7-43D0-4765-97CA-74C126665F3E}.Release|x86.ActiveCfg = Release|Win32
|
||||
{DF0881C7-43D0-4765-97CA-74C126665F3E}.Release|x86.Build.0 = Release|Win32
|
||||
{A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Debug|x64.ActiveCfg = Debug|x64
|
||||
{A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Debug|x64.Build.0 = Debug|x64
|
||||
{A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
{A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Debug|x86.Build.0 = Debug|Win32
|
||||
{A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Release|x64.ActiveCfg = Release|x64
|
||||
{A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Release|x64.Build.0 = Release|x64
|
||||
{A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Release|x86.ActiveCfg = Release|Win32
|
||||
{A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Release|x86.Build.0 = Release|Win32
|
||||
{E5FAAACF-703C-4460-93B8-301991F62778}.Debug|x64.ActiveCfg = Debug|x64
|
||||
{E5FAAACF-703C-4460-93B8-301991F62778}.Debug|x64.Build.0 = Debug|x64
|
||||
{E5FAAACF-703C-4460-93B8-301991F62778}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
{E5FAAACF-703C-4460-93B8-301991F62778}.Debug|x86.Build.0 = Debug|Win32
|
||||
{E5FAAACF-703C-4460-93B8-301991F62778}.Release|x64.ActiveCfg = Release|x64
|
||||
{E5FAAACF-703C-4460-93B8-301991F62778}.Release|x64.Build.0 = Release|x64
|
||||
{E5FAAACF-703C-4460-93B8-301991F62778}.Release|x86.ActiveCfg = Release|Win32
|
||||
{E5FAAACF-703C-4460-93B8-301991F62778}.Release|x86.Build.0 = Release|Win32
|
||||
EndGlobalSection
|
||||
GlobalSection(SolutionProperties) = preSolution
|
||||
HideSolutionNode = FALSE
|
||||
|
||||
@ -484,7 +484,7 @@ typedef struct
|
||||
int b;
|
||||
}SG_color;
|
||||
void _outputScanDataFile_RGBD_obj_XYZ(char* fileName, SVzNL3DLaserLine* scanData, int lineNum,
|
||||
float lineV, int maxTimeStamp, int clockPerSecond, std::vector<std::vector<SVzNL3DPoint>>& objOps)
|
||||
float lineV, int maxTimeStamp, int clockPerSecond, std::vector<SVzNL3DPoint>& objOps)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
int realLines = lineNum;
|
||||
@ -605,46 +605,35 @@ void _outputScanDataFile_RGBD_obj_XYZ(char* fileName, SVzNL3DLaserLine* scanData
|
||||
}
|
||||
if (objOps.size() > 0)
|
||||
{
|
||||
int objNum = 0;
|
||||
for (int i = 0, i_max = objOps.size(); i < i_max; i++)
|
||||
int ptNum = objOps.size();
|
||||
sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << ptNum << std::endl;
|
||||
for (int i = 0; i < objOps.size(); i++)
|
||||
{
|
||||
std::vector<SVzNL3DPoint>& obj_line = objOps[i];
|
||||
objNum += obj_line.size();
|
||||
}
|
||||
if (i == 0)
|
||||
rgb = { 255, 0, 0 };
|
||||
else
|
||||
rgb = { 255, 255, 0 };
|
||||
size = 25;
|
||||
float x = (float)objOps[i].x;
|
||||
float y = (float)objOps[i].y;
|
||||
float z = (float)objOps[i].z;
|
||||
|
||||
sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << objNum << std::endl;
|
||||
for (int i = 0, i_max = objOps.size(); i < i_max; i++)
|
||||
{
|
||||
std::vector<SVzNL3DPoint>& obj_line = objOps[i];
|
||||
for (int j = 0, j_max = obj_line.size(); j < j_max; j++)
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
if (i == 0)
|
||||
{
|
||||
rgb = objColor[i%8];
|
||||
size = 25;
|
||||
float x = (float)obj_line[j].x;
|
||||
float y = (float)obj_line[j].y;
|
||||
float z = (float)obj_line[j].z;
|
||||
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
if (i == 0)
|
||||
{
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
sw.close();
|
||||
}
|
||||
|
||||
void _XOYprojection_nodePos(
|
||||
cv::Mat& img,
|
||||
std::vector<std::vector< SVzNL3DPosition>>& dataLines,
|
||||
std::vector<std::vector<SVzNL3DPoint>>& objOps,
|
||||
const double x_scale, const double y_scale,
|
||||
const SVzNLRangeD x_range, const SVzNLRangeD y_range)
|
||||
void _XOYprojection_nodePos(cv::Mat& img, std::vector<std::vector< SVzNL3DPosition>>& dataLines, std::vector<SVzNL3DPoint>& objOps,
|
||||
const double x_scale, const double y_scale, const SVzNLRangeD x_range, const SVzNLRangeD y_range)
|
||||
{
|
||||
int x_skip = 16;
|
||||
int y_skip = 16;
|
||||
@ -759,24 +748,27 @@ void _XOYprojection_nodePos(
|
||||
}
|
||||
if (objOps.size() > 0)
|
||||
{
|
||||
for (int i = 0, i_max = objOps.size(); i < i_max; i++)
|
||||
for (int i = 0; i < objOps.size(); i++)
|
||||
{
|
||||
std::vector<SVzNL3DPoint>& obj_line = objOps[i];
|
||||
for (int j = 0, j_max = obj_line.size(); j < j_max; j++)
|
||||
if (i == 0)
|
||||
{
|
||||
rgb = objColor[i % 8];
|
||||
rgb = { 255, 0, 0 };
|
||||
size = 20;
|
||||
|
||||
int px = (int)((obj_line[j].x - x_range.min) / x_scale + x_skip);
|
||||
int py = (int)((obj_line[j].y - y_range.min) / y_scale + y_skip);
|
||||
cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1);
|
||||
}
|
||||
else
|
||||
{
|
||||
rgb = { 255, 255, 0 };
|
||||
size = 10;
|
||||
}
|
||||
int px = (int)((objOps[i].x - x_range.min) / x_scale + x_skip);
|
||||
int py = (int)((objOps[i].y - y_range.min) / y_scale + y_skip);
|
||||
cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void _genXOYProjectionImage_nodePos(cv::String& fileName, SVzNL3DLaserLine* scanData, int lineNum, std::vector<std::vector<SVzNL3DPoint>>& nodeOps)
|
||||
void _genXOYProjectionImage_nodePos(cv::String& fileName, SVzNL3DLaserLine* scanData, int lineNum, std::vector<SVzNL3DPoint>& nodeOps)
|
||||
{
|
||||
//统计X和Y的范围
|
||||
std::vector<std::vector< SVzNL3DPosition>> scan_lines;
|
||||
@ -919,160 +911,12 @@ SSG_planeCalibPara _readCalibPara(char* fileName)
|
||||
return planePara;
|
||||
}
|
||||
|
||||
std::vector<SVzNL3DPoint> _wdReadLMIData_TXT(char* fileName)
|
||||
{
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
|
||||
std::vector<SVzNL3DPoint> resultData;
|
||||
if (inputFile.is_open() == false)
|
||||
return resultData;
|
||||
|
||||
while (getline(inputFile, linedata))
|
||||
{
|
||||
double x, y, z;
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &y, &x, &z);
|
||||
SVzNL3DPoint a_pt;
|
||||
a_pt.x = x;
|
||||
a_pt.y = y;
|
||||
a_pt.z = z;
|
||||
resultData.push_back(a_pt);
|
||||
}
|
||||
return resultData;
|
||||
}
|
||||
|
||||
std::vector<SVzNL3DPoint> _wdReadGuangZiData_TXT(char* fileName, double tick_scale, double offset)
|
||||
{
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
|
||||
std::vector<SVzNL3DPoint> resultData;
|
||||
if (inputFile.is_open() == false)
|
||||
return resultData;
|
||||
|
||||
while (getline(inputFile, linedata))
|
||||
{
|
||||
double x, y, z;
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &x, &y, &z);
|
||||
if (z > -9999.0 + 1e-4) //1e-4为防止float误差
|
||||
{
|
||||
SVzNL3DPoint a_pt;
|
||||
a_pt.x = (x - offset)* tick_scale;
|
||||
a_pt.y = y;
|
||||
a_pt.z = z;
|
||||
resultData.push_back(a_pt);
|
||||
}
|
||||
}
|
||||
return resultData;
|
||||
}
|
||||
|
||||
SVzNL3DLaserLine* _convertToVizumDataFormat(std::vector<SVzNL3DPoint>& ptList, int* vldLineNum)
|
||||
{
|
||||
|
||||
std::vector<std::vector< SVzNL3DPoint>> lineList;
|
||||
std::vector< SVzNL3DPoint> a_line;
|
||||
double pre_y = 1.0e+10;;
|
||||
int maxLineSize = 0;
|
||||
for (int i = 0, i_max = ptList.size(); i < i_max; i++)
|
||||
{
|
||||
if (ptList[i].y < pre_y-1.0) //1.0为容错门限，防止相邻点有反转
|
||||
{
|
||||
//新行开始
|
||||
if (a_line.size() > 0)
|
||||
{
|
||||
lineList.push_back(a_line);
|
||||
if (maxLineSize < a_line.size())
|
||||
maxLineSize = a_line.size();
|
||||
a_line.clear();
|
||||
}
|
||||
a_line.push_back(ptList[i]);
|
||||
}
|
||||
else
|
||||
a_line.push_back(ptList[i]);
|
||||
pre_y = ptList[i].y;
|
||||
}
|
||||
//最后一行
|
||||
if (a_line.size() > 0)
|
||||
{
|
||||
lineList.push_back(a_line);
|
||||
if (maxLineSize < a_line.size())
|
||||
maxLineSize = a_line.size();
|
||||
}
|
||||
|
||||
int lineNum = lineList.size();
|
||||
*vldLineNum = lineNum;
|
||||
SVzNL3DLaserLine* resultdData = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * lineNum);
|
||||
memset(resultdData, 0, sizeof(SVzNL3DLaserLine) * lineNum);
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
std::vector< SVzNL3DPoint>& a_lineData = lineList[line];
|
||||
int pt_counter = a_lineData.size();
|
||||
resultdData[line].nPositionCnt = pt_counter;
|
||||
resultdData[line].nTimeStamp = 0;
|
||||
resultdData[line].p3DPosition = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * pt_counter);
|
||||
memset(resultdData[line].p3DPosition, 0, sizeof(SVzNL3DPosition) * pt_counter);
|
||||
for (int i = 0; i < pt_counter; i++)
|
||||
{
|
||||
resultdData[line].p3DPosition[i].nPointIdx = i;
|
||||
resultdData[line].p3DPosition[i].pt3D = a_lineData[i];
|
||||
}
|
||||
}
|
||||
return resultdData;
|
||||
}
|
||||
|
||||
|
||||
#define TEST_CONVERT_TO_GRID 0
|
||||
#define TEST_COMPUTE_SIEVE_NODES 1
|
||||
#define TEST_COMPUTE_CALIB_PARA 0
|
||||
#define TEST_CONVERT_LMI_DATA 0
|
||||
#define TEST_CONVERT_GUANGZI_DATA 0
|
||||
#define TEST_GROUP 2
|
||||
int main()
|
||||
{
|
||||
#if TEST_CONVERT_LMI_DATA
|
||||
//将数据转换成栅格格式格式
|
||||
int lineNum = 0;
|
||||
float lineV = 0.0f;
|
||||
int dataCalib = 0;
|
||||
int maxTimeStamp = 0;
|
||||
int clockPerSecond = 0;
|
||||
char _LMI_file[256];
|
||||
sprintf_s(_LMI_file, "F:\\张奔-标定采图\\钢块-标定采图\\5-右下-2.txt");
|
||||
std::vector<SVzNL3DPoint> LMI_data = _wdReadLMIData_TXT(_LMI_file);
|
||||
if (LMI_data.size() == 0)
|
||||
return 0;
|
||||
|
||||
int vldLineNum = 0;
|
||||
SVzNL3DLaserLine* convertData = _convertToVizumDataFormat(LMI_data, &vldLineNum);
|
||||
char _out_file[256];
|
||||
sprintf_s(_out_file, "F:\\张奔-标定采图\\钢块-标定采图\\5-右下-2_onverted.txt");
|
||||
_outputScanDataFile_self(_out_file, convertData, vldLineNum,
|
||||
lineV, maxTimeStamp, clockPerSecond);
|
||||
printf("%s: convert done!\n", _LMI_file);
|
||||
#endif
|
||||
#if TEST_CONVERT_GUANGZI_DATA
|
||||
//将数据转换成栅格格式格式
|
||||
int lineNum = 0;
|
||||
float lineV = 0.0f;
|
||||
int dataCalib = 0;
|
||||
int maxTimeStamp = 0;
|
||||
int clockPerSecond = 0;
|
||||
char _Guangzi_file[256];
|
||||
double offset = 300000;
|
||||
double tick_scale = 0.05; //左正
|
||||
sprintf_s(_Guangzi_file, "F:\\张奔-标定采图\\钢块-标定采图\\5-右正-2.txt");
|
||||
std::vector<SVzNL3DPoint> Guangzi_data = _wdReadGuangZiData_TXT(_Guangzi_file, tick_scale, offset);
|
||||
if (Guangzi_data.size() == 0)
|
||||
return 0;
|
||||
|
||||
int vldLineNum = 0;
|
||||
SVzNL3DLaserLine* convertData = _convertToVizumDataFormat(Guangzi_data, &vldLineNum);
|
||||
char _out_file[256];
|
||||
sprintf_s(_out_file, "F:\\张奔-标定采图\\钢块-标定采图\\5-右正-2_onverted.txt");
|
||||
_outputScanDataFile_self(_out_file, convertData, vldLineNum,
|
||||
lineV, maxTimeStamp, clockPerSecond);
|
||||
printf("%s: convert done!\n", _Guangzi_file);
|
||||
#endif
|
||||
#if TEST_CONVERT_TO_GRID
|
||||
//将数据转换成栅格格式格式
|
||||
char _scan_dir[256];
|
||||
@ -1267,7 +1111,7 @@ int main()
|
||||
sg_lineDataR(&laser3DPoints[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight);
|
||||
}
|
||||
//计算节点位置
|
||||
std::vector<std::vector<SVzNL3DPoint>> nodePos;
|
||||
std::vector<SVzNL3DPoint> nodePos;
|
||||
sg_getSieveNodes(
|
||||
laser3DPoints,
|
||||
lineNum,
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -166,7 +166,6 @@
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="..\sourceCode\SG_baseFunc.cpp" />
|
||||
<ClCompile Include="..\sourceCode\SG_clustering.cpp" />
|
||||
<ClCompile Include="..\sourceCode\SG_featureGrow.cpp" />
|
||||
<ClCompile Include="..\sourceCode\SG_lineFeature.cpp" />
|
||||
<ClCompile Include="..\sourceCode\SG_regionGrow.cpp" />
|
||||
|
||||
@ -12,13 +12,6 @@ void sg_lineDataR(SVzNL3DLaserLine* a_line,
|
||||
lineDataRT( a_line, camPoseR, groundH);
|
||||
}
|
||||
|
||||
void sg_lineDataR_RGBD(SVzNLXYZRGBDLaserLine* a_line,
|
||||
const double* camPoseR,
|
||||
double groundH)
|
||||
{
|
||||
lineDataRT_RGBD(a_line, camPoseR, groundH);
|
||||
}
|
||||
|
||||
//扫描线处理，进行垂直方向的特征提取和生长
|
||||
void sg_bagPositioning_lineProc(
|
||||
SVzNL3DLaserLine* a_line,
|
||||
@ -2296,7 +2289,7 @@ void sg_getBagPosition(
|
||||
cv::imwrite("originMask.png", oriMaskImage);
|
||||
#endif
|
||||
|
||||
int vldRgnSize = (int)((algoParam.bagParam.bagL * algoParam.bagParam.bagW / 2) / (scale * scale));
|
||||
int vldRgnSize = (int)((algoParam.bagParam.bagL / 2) * (algoParam.bagParam.bagW / 2) / (scale * scale));
|
||||
for (int rgnid = 0, rgn_max = labelRgns.size(); rgnid < rgn_max; rgnid++)
|
||||
{
|
||||
SSG_Region* a_rgn = &labelRgns[rgnid];
|
||||
@ -3814,440 +3807,6 @@ if (i == 2)
|
||||
}
|
||||
}
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int vldNum[RGN_HIST_SIZE];
|
||||
int totalNum[RGN_HIST_SIZE];
|
||||
}_rgnHSVHistInfo;
|
||||
|
||||
HSV RGBtoHSV(int r, int g, int b) {
|
||||
double r_ = r / 255.0;
|
||||
double g_ = g / 255.0;
|
||||
double b_ = b / 255.0;
|
||||
double max_ = std::max(r_, std::max(g_, b_));
|
||||
double min_ = std::min(r_, std::min(g_, b_));
|
||||
double delta = max_ - min_;
|
||||
double h, s, v;
|
||||
v = max_;
|
||||
s = (max_ == 0) ? 0 : (delta / max_);
|
||||
if (delta == 0) //此处有修改。设定一个门限
|
||||
{
|
||||
h = -1; // 未定义，可以是任何值，通常设为0或NaN
|
||||
}
|
||||
else
|
||||
{
|
||||
if (r_ == max_) {
|
||||
h = (g_ - b_) / delta;
|
||||
}
|
||||
else if (g_ == max_) {
|
||||
h = 2 + (b_ - r_) / delta;
|
||||
}
|
||||
else { // b_ == max_
|
||||
h = 4 + (r_ - g_) / delta;
|
||||
}
|
||||
h *= 60; // 将h的值标准化到[0,360]度
|
||||
if (h < 0) {
|
||||
h += 360; // 确保h在[0,360]范围内
|
||||
}
|
||||
}
|
||||
HSV hsv;
|
||||
hsv.h = h;
|
||||
hsv.s = s * 255;// 将s的值标准化到[0,255]
|
||||
hsv.v = v * 255;// 将v的值标准化到[0,255]
|
||||
return hsv;
|
||||
}
|
||||
|
||||
bool isSameColor(HSV hsvPattern, HSV hsvPt, const SSG_hsvCmpParam colorCmpParam)
|
||||
{
|
||||
if (((hsvPattern.h < 0) && (hsvPt.h >= 0)) || ((hsvPattern.h >= 0) && (hsvPt.h < 0)))
|
||||
return false;
|
||||
|
||||
if ((hsvPattern.h < 0) && (hsvPt.h < 0))
|
||||
return true;
|
||||
|
||||
double h_diff = hsvPt.h - hsvPt.h;
|
||||
if (h_diff < -180)
|
||||
h_diff = h_diff + 360;
|
||||
else if (h_diff > 180)
|
||||
h_diff = h_diff - 360;
|
||||
h_diff = abs(h_diff);
|
||||
double s_diff = abs(hsvPattern.s - hsvPt.s);
|
||||
if ( (h_diff < colorCmpParam.hueTh) && (s_diff < colorCmpParam.saturateTh))
|
||||
return true;
|
||||
else
|
||||
return false;
|
||||
}
|
||||
|
||||
double cosSimilarity(const double* colorTemplate, double* sampleData)
|
||||
{
|
||||
double sumA = 0, sumB = 0, sumAB = 0;
|
||||
for (int i = 0; i < RGN_HIST_SIZE; i++)
|
||||
{
|
||||
sumA += colorTemplate[i] * colorTemplate[i];
|
||||
sumAB += colorTemplate[i] * sampleData[i];
|
||||
sumB = sampleData[i] * sampleData[i];
|
||||
}
|
||||
sumA = sqrt(sumA);
|
||||
sumB = sqrt(sumB);
|
||||
sumA = sumA * sumB;
|
||||
if (sumA <= 1e-4)
|
||||
return 0;
|
||||
return (sumAB / sumA);
|
||||
}
|
||||
|
||||
double cosSimilarityInv(const double* colorTemplate, double* sampleData)
|
||||
{
|
||||
double sumA = 0, sumB = 0, sumAB = 0;
|
||||
for (int i = 0; i < RGN_HIST_SIZE; i++)
|
||||
{
|
||||
sumA += colorTemplate[i] * colorTemplate[i];
|
||||
sumAB += colorTemplate[i] * sampleData[RGN_HIST_SIZE - 1 - i];
|
||||
sumB = sampleData[RGN_HIST_SIZE - 1 - i] * sampleData[RGN_HIST_SIZE -1 - i];
|
||||
}
|
||||
sumA = sqrt(sumA);
|
||||
sumB = sqrt(sumB);
|
||||
sumA = sumA * sumB;
|
||||
if (sumA <= 1e-4)
|
||||
return 0;
|
||||
return (sumAB / sumA);
|
||||
}
|
||||
|
||||
///数据输入必须是RGBD grid格式，以进行水平方向和垂直方向的处理
|
||||
///（1）寻找边界点
|
||||
///（2）从最高点开始进行区域生长
|
||||
/// 输出编织袋及袋子上下朝向
|
||||
void sg_getBagPositionAndOrientation(
|
||||
SVzNLXYZRGBDLaserLine* laser3DPoints,
|
||||
int lineNum,
|
||||
//std::vector<SSG_lineFeature>& all_vLineFeatures,
|
||||
//std::vector<std::vector<int>>& noisePts,
|
||||
const SG_bagPositionParam algoParam,
|
||||
const SSG_planeCalibPara poseCalibPara,
|
||||
const SSG_hsvCmpParam colorCmpParam,
|
||||
const RGB rgbColorPattern, //用于区分袋子方向的颜色
|
||||
const double frontColorTemplate[RGN_HIST_SIZE],
|
||||
const double backColorTemplate[RGN_HIST_SIZE],
|
||||
std::vector<SSG_peakOrienRgnInfo>& objOps,
|
||||
#if OUTPUT_DEBUG
|
||||
std::vector<std::vector< SVzNL3DPosition>>& bagPositionCloudPts,
|
||||
#endif
|
||||
int* errCode)
|
||||
{
|
||||
*errCode = 0;
|
||||
if (lineNum == 0)
|
||||
{
|
||||
*errCode = SG_ERR_3D_DATA_INVLD;
|
||||
return;
|
||||
}
|
||||
//检查是否为栅格格式
|
||||
//将RGBD点云分离出对应的RGB和点云，RGB转成HSV
|
||||
std::vector<SVzNL3DLaserLine> cloudPts;
|
||||
std::vector<std::vector<HSV>> hsvPts;
|
||||
bool dataValid = true;
|
||||
int linePtNum = laser3DPoints[0].nPointCnt;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
if (laser3DPoints[line].nPointCnt > 0)
|
||||
{
|
||||
std::vector<HSV> line_hsv;
|
||||
line_hsv.resize(laser3DPoints[line].nPointCnt);
|
||||
SVzNL3DPosition* p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * laser3DPoints[line].nPointCnt);
|
||||
memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * laser3DPoints[line].nPointCnt);
|
||||
for (int i = 0; i < laser3DPoints[line].nPointCnt; i++)
|
||||
{
|
||||
if ((line == 497) && (i >= 902))
|
||||
int kkk = 1;
|
||||
p3DPoint[i].pt3D.x = laser3DPoints[line].p3DPoint[i].x;
|
||||
p3DPoint[i].pt3D.y = laser3DPoints[line].p3DPoint[i].y;
|
||||
p3DPoint[i].pt3D.z = laser3DPoints[line].p3DPoint[i].z;
|
||||
p3DPoint[i].nPointIdx = i;
|
||||
|
||||
unsigned int nRGB = laser3DPoints[line].p3DPoint[i].nRGB;
|
||||
|
||||
int r = nRGB & 0xff;
|
||||
nRGB >>= 8;
|
||||
int g = nRGB & 0xff;
|
||||
nRGB >>= 8;
|
||||
int b = nRGB & 0xff;
|
||||
line_hsv[i] = RGBtoHSV(r,g,b);
|
||||
}
|
||||
|
||||
SVzNL3DLaserLine a_line;
|
||||
a_line.nPositionCnt = laser3DPoints[line].nPointCnt;
|
||||
a_line.nTimeStamp = 0;
|
||||
a_line.p3DPosition = p3DPoint;
|
||||
cloudPts.push_back(a_line);
|
||||
hsvPts.push_back(line_hsv);
|
||||
}
|
||||
if (linePtNum != laser3DPoints[line].nPointCnt)
|
||||
dataValid = false;
|
||||
}
|
||||
|
||||
if (false == dataValid)
|
||||
{
|
||||
*errCode = SG_ERR_3D_DATA_INVLD;
|
||||
//释放内存
|
||||
for (int i = 0, i_max = cloudPts.size(); i < i_max; i++)
|
||||
free(cloudPts[i].p3DPosition);
|
||||
return;
|
||||
}
|
||||
|
||||
//根据目标
|
||||
std::vector<SSG_peakRgnInfo> peakRgn;
|
||||
sg_getBagPosition(
|
||||
&cloudPts[0],
|
||||
cloudPts.size(),
|
||||
algoParam,
|
||||
poseCalibPara,
|
||||
peakRgn);
|
||||
|
||||
//释放内存
|
||||
for (int i = 0, i_max = cloudPts.size(); i < i_max; i++)
|
||||
{
|
||||
#if OUTPUT_DEBUG
|
||||
std::vector< SVzNL3DPosition> out_line;
|
||||
int nPtCounter = cloudPts[i].nPositionCnt;
|
||||
for (int j = 0; j < nPtCounter; j++)
|
||||
out_line.push_back(cloudPts[i].p3DPosition[j]);
|
||||
bagPositionCloudPts.push_back(out_line);
|
||||
#endif
|
||||
free(cloudPts[i].p3DPosition);
|
||||
cloudPts[i].p3DPosition = NULL;
|
||||
}
|
||||
|
||||
//将数据重新投射回原来的坐标系，以保持手眼标定结果正确
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
sg_lineDataR_RGBD(&laser3DPoints[i], poseCalibPara.invRMatrix, -1);
|
||||
|
||||
//将颜色属性转成HSV
|
||||
HSV hsvPattern = RGBtoHSV(rgbColorPattern.r, rgbColorPattern.g, rgbColorPattern.b);
|
||||
//进行颜色统计处理：取centerPos为中心的（0.8*Width, 0.8*Height）的范围内的点进行统计
|
||||
int objNum = peakRgn.size();
|
||||
if (objNum > 0)
|
||||
{
|
||||
//以水平放置的矩阵为标准模型位置，即水平方向为长度，垂直方向为宽度
|
||||
std::vector<double> y_th;
|
||||
y_th.resize(objNum);
|
||||
std::vector<double> x_th;
|
||||
x_th.resize(objNum);
|
||||
std::vector<double> x_slice;
|
||||
x_slice.resize(objNum);
|
||||
for (int m = 0; m < objNum; m++)
|
||||
{
|
||||
y_th[m] = peakRgn[m].objSize.dHeight * 0.25;
|
||||
x_th[m] = peakRgn[m].objSize.dWidth * 0.45;
|
||||
x_slice[m] = peakRgn[m].objSize.dWidth / 16.0;
|
||||
}
|
||||
|
||||
std::vector<SSG_ROIRectD> rgnROIs;
|
||||
rgnROIs.resize(objNum);
|
||||
for (int i = 0; i < objNum; i++)
|
||||
{
|
||||
//计算一个大的ROI，加速处理
|
||||
//以水平放置的矩阵为标准模型位置
|
||||
SVzNL2DPointD vec[4];
|
||||
vec[0].x = -peakRgn[i].objSize.dWidth / 2;
|
||||
vec[0].y = -peakRgn[i].objSize.dHeight / 2;
|
||||
vec[1].x = peakRgn[i].objSize.dWidth / 2;
|
||||
vec[1].y = -peakRgn[i].objSize.dHeight / 2;
|
||||
vec[2].x = peakRgn[i].objSize.dWidth / 2;
|
||||
vec[2].y = peakRgn[i].objSize.dHeight / 2;
|
||||
vec[3].x = -peakRgn[i].objSize.dWidth / 2;
|
||||
vec[3].y = peakRgn[i].objSize.dHeight / 2;
|
||||
//旋转
|
||||
const double deg2rad = PI / 180.0;
|
||||
const double yaw = peakRgn[i].centerPos.z_yaw * deg2rad;
|
||||
double cy = cos(yaw); double sy = sin(yaw);
|
||||
for (int m = 0; m < 4; m++)
|
||||
{
|
||||
double x = vec[m].x * cy + vec[m].y * sy; //图像坐标系的Y与欧氏坐标系方向相反，
|
||||
double y = -vec[m].x * sy + vec[m].y * cy;
|
||||
vec[m].x = x + peakRgn[i].centerPos.x;
|
||||
vec[m].y = y + peakRgn[i].centerPos.y;
|
||||
}
|
||||
//生成ROI
|
||||
SSG_ROIRectD a_roi;
|
||||
a_roi.left = vec[0].x;
|
||||
a_roi.right = vec[0].x;
|
||||
a_roi.top = vec[0].y;
|
||||
a_roi.bottom = vec[0].y;
|
||||
for (int m = 1; m < 4; m++)
|
||||
{
|
||||
a_roi.left = a_roi.left > vec[m].x ? vec[m].x : a_roi.left;
|
||||
a_roi.right = a_roi.right < vec[m].x ? vec[m].x : a_roi.right;
|
||||
a_roi.top = a_roi.top > vec[m].y ? vec[m].y : a_roi.top;
|
||||
a_roi.bottom = a_roi.bottom < vec[m].y ? vec[m].y : a_roi.bottom;
|
||||
}
|
||||
rgnROIs[i] = a_roi;
|
||||
}
|
||||
|
||||
std::vector<_rgnHSVHistInfo> rgnHists; //统计量
|
||||
rgnHists.resize(objNum);
|
||||
//统计量清零
|
||||
for (int m = 0; m < objNum; m++)
|
||||
{
|
||||
for (int j = 0; j < 16; j++)
|
||||
{
|
||||
rgnHists[m].totalNum[j] = 0;
|
||||
rgnHists[m].vldNum[j] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
//对所有点进投射，注意此处使用原始点
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
{
|
||||
SVzNLPointXYZRGBA* a_pt = &laser3DPoints[line].p3DPoint[j];
|
||||
if (a_pt->z < 1e-4)
|
||||
continue;
|
||||
if ((line == 497) && (j >= 902))
|
||||
{
|
||||
HSV a_hsv = hsvPts[line][j];
|
||||
int kkk = 1;
|
||||
}
|
||||
for (int m = 0; m < objNum; m++)
|
||||
{
|
||||
//检查点是否在ROI内
|
||||
if ((a_pt->x >= rgnROIs[m].left) && (a_pt->x <= rgnROIs[m].right) &&
|
||||
(a_pt->y >= rgnROIs[m].top) && (a_pt->y <= rgnROIs[m].bottom))
|
||||
{
|
||||
//计算投射点
|
||||
//反向旋转,逆时针角度
|
||||
const double deg2rad = PI / 180.0;
|
||||
double rotateAngle = - peakRgn[m].centerPos.z_yaw * deg2rad;
|
||||
double cy = cos(rotateAngle); double sy = sin(rotateAngle);
|
||||
double x = a_pt->x - peakRgn[m].centerPos.x;
|
||||
double y = a_pt->y - peakRgn[m].centerPos.y;
|
||||
double rx = x * cy + y * sy;
|
||||
double ry = -x * sy + y * cy;
|
||||
if ((rx >= -x_th[m]) && (rx <= x_th[m]) && (ry >= -y_th[m]) && (ry <= y_th[m]))
|
||||
{
|
||||
if (m == 0)
|
||||
int kkk = 1;
|
||||
else if (m == 1)
|
||||
int kkk = 1;
|
||||
else if (m == 2)
|
||||
int kkk = 1;
|
||||
else if (m == 3)
|
||||
int kkk = 1;
|
||||
double rx_abs = abs(rx);
|
||||
int hist_idx = rx_abs / x_slice[m];
|
||||
if (hist_idx >= RGN_HIST_SIZE / 2)
|
||||
hist_idx = RGN_HIST_SIZE / 2 - 1;
|
||||
if (rx < 0)
|
||||
hist_idx = -hist_idx + RGN_HIST_SIZE / 2 - 1;
|
||||
else
|
||||
hist_idx = hist_idx + RGN_HIST_SIZE / 2;
|
||||
//判断属性
|
||||
bool isSame = isSameColor(hsvPattern, hsvPts[line][j], colorCmpParam);
|
||||
if (true == isSame)
|
||||
rgnHists[m].vldNum[hist_idx] ++;
|
||||
rgnHists[m].totalNum[hist_idx] ++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
for (int m = 0; m < objNum; m++)
|
||||
{
|
||||
double normalizeHist[RGN_HIST_SIZE];
|
||||
//生成归一化数据
|
||||
int totalNum=0, totalVldNum=0, upVldNum=0, dwnVldNum=0;
|
||||
for (int j = 0; j < RGN_HIST_SIZE; j++)
|
||||
{
|
||||
totalNum += rgnHists[m].totalNum[j];
|
||||
totalVldNum += rgnHists[m].vldNum[j];
|
||||
|
||||
if (j < (RGN_HIST_SIZE / 2-2))
|
||||
dwnVldNum += rgnHists[m].vldNum[j];
|
||||
else if (j >= (RGN_HIST_SIZE / 2+2))
|
||||
upVldNum += rgnHists[m].vldNum[j];
|
||||
|
||||
if (rgnHists[m].totalNum[j] > 0)
|
||||
normalizeHist[j] = (double)rgnHists[m].vldNum[j] / (double)rgnHists[m].totalNum[j];
|
||||
else
|
||||
normalizeHist[j] = 0;
|
||||
}
|
||||
|
||||
SSG_peakOrienRgnInfo a_obj;
|
||||
a_obj.centerPos = peakRgn[m].centerPos;
|
||||
a_obj.objSize = peakRgn[m].objSize;
|
||||
a_obj.pkRgnIdx = peakRgn[m].pkRgnIdx;
|
||||
a_obj.pos2D = peakRgn[m].pos2D;
|
||||
a_obj.orienFlag = 0; //未知正反
|
||||
//相似度比较
|
||||
#if 0
|
||||
double frontSim = cosSimilarity(frontColorTemplate, normalizeHist);
|
||||
double frontSim_inv = cosSimilarityInv(frontColorTemplate, normalizeHist);
|
||||
double backSim = cosSimilarity(backColorTemplate, normalizeHist);
|
||||
double backSim_inv = cosSimilarityInv(backColorTemplate, normalizeHist);
|
||||
if ((frontSim > frontSim_inv) && (frontSim > backSim) && (frontSim > backSim_inv) && (frontSim > 0.5))
|
||||
{
|
||||
a_obj.orienFlag = 1;
|
||||
}
|
||||
else if ((frontSim_inv > frontSim) && (frontSim_inv > backSim) && (frontSim_inv > backSim_inv) && (frontSim_inv > 0.5))
|
||||
{
|
||||
a_obj.orienFlag = 1;
|
||||
a_obj.centerPos.z_yaw += 180;
|
||||
if (a_obj.centerPos.z_yaw >= 360)
|
||||
a_obj.centerPos.z_yaw = a_obj.centerPos.z_yaw - 360;
|
||||
}
|
||||
else if ((backSim > frontSim) && (backSim > frontSim_inv) && (backSim > backSim_inv) && (backSim > 0.5))
|
||||
{
|
||||
a_obj.orienFlag = 2;
|
||||
}
|
||||
else if ((backSim_inv > frontSim) && (backSim_inv > frontSim_inv) && (backSim_inv > backSim) && (backSim_inv > 0.5))
|
||||
{
|
||||
a_obj.orienFlag = 2;
|
||||
a_obj.centerPos.z_yaw += 180;
|
||||
if (a_obj.centerPos.z_yaw >= 360)
|
||||
a_obj.centerPos.z_yaw = a_obj.centerPos.z_yaw - 360;
|
||||
}
|
||||
#else
|
||||
double vldPtRatio = (double)totalVldNum / (double)totalNum;
|
||||
bool isFront;
|
||||
if (true == colorCmpParam.frontVldPtGreater)
|
||||
isFront = vldPtRatio > colorCmpParam.FBVldPtRatioTh ? true : false;
|
||||
else
|
||||
isFront = vldPtRatio > colorCmpParam.FBVldPtRatioTh ? false : true;
|
||||
|
||||
bool orienIsNormal;
|
||||
if (true == isFront)
|
||||
{
|
||||
if (true == colorCmpParam.front_upVldPtGreater)
|
||||
orienIsNormal = upVldNum > dwnVldNum ? true : false;
|
||||
else
|
||||
orienIsNormal = upVldNum > dwnVldNum ? false : true;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (true == colorCmpParam.back_upVldPtGreater)
|
||||
orienIsNormal = upVldNum > dwnVldNum ? true : false;
|
||||
else
|
||||
orienIsNormal = upVldNum > dwnVldNum ? false : true;
|
||||
}
|
||||
|
||||
if (true == isFront)
|
||||
a_obj.orienFlag = 1;
|
||||
else
|
||||
a_obj.orienFlag = 2;
|
||||
|
||||
if (false == orienIsNormal)
|
||||
{
|
||||
a_obj.centerPos.z_yaw += 180;
|
||||
if (a_obj.centerPos.z_yaw >= 360)
|
||||
a_obj.centerPos.z_yaw = a_obj.centerPos.z_yaw - 360;
|
||||
}
|
||||
#endif
|
||||
objOps.push_back(a_obj);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
bool compareByXPos(const SSG_featureTree& a, const SSG_featureTree& b) {
|
||||
return (a.roi.left + a.roi.right) < (b.roi.left + b.roi.right);
|
||||
}
|
||||
|
||||
@ -6,9 +6,8 @@
|
||||
# define SG_APISHARED_EXPORT __declspec(dllimport)
|
||||
#endif
|
||||
|
||||
#define OUTPUT_DEBUG 1
|
||||
#define OUTPUT_DEBUG 0
|
||||
#define BAG_ALGO_USE_CORNER_FEATURE 1
|
||||
#define RGN_HIST_SIZE 16 //目标颜色统计的数目
|
||||
|
||||
#include "SG_baseDataType.h"
|
||||
#include <vector>
|
||||
@ -28,16 +27,6 @@ typedef struct
|
||||
//SSG_polarScanParam polarScanParam;
|
||||
}SG_bagPositionParam;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double hueTh;
|
||||
double saturateTh;
|
||||
double FBVldPtRatioTh; //正反两面有效颜色点的比例门限
|
||||
bool frontVldPtGreater; //true：有效颜色比例高的点的是正面；false：有效颜色比例高的点的是反面
|
||||
bool front_upVldPtGreater;//true：有效颜色比例高的点的是正面朝上；false：有效颜色比例高的点的是正面朝下
|
||||
bool back_upVldPtGreater; //true：有效颜色比例高的点的是反面朝上；false：有效颜色比例高的点的是反面朝下
|
||||
}SSG_hsvCmpParam;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double jumpTh; //和现场环境有关，为托盘的孔洞的Z跳变
|
||||
@ -45,13 +34,7 @@ typedef struct
|
||||
double baseHoleDist; //托盘两个孔洞的距离
|
||||
}SSG_stackBaseParam;
|
||||
|
||||
//数据调平
|
||||
SG_APISHARED_EXPORT void sg_lineDataR(
|
||||
SVzNL3DLaserLine* a_line,
|
||||
const double* camPoseR,
|
||||
double groundH);
|
||||
SG_APISHARED_EXPORT void sg_lineDataR_RGBD(
|
||||
SVzNLXYZRGBDLaserLine* a_line,
|
||||
SG_APISHARED_EXPORT void sg_lineDataR(SVzNL3DLaserLine* a_line,
|
||||
const double* camPoseR,
|
||||
double groundH);
|
||||
|
||||
@ -73,23 +56,6 @@ SG_APISHARED_EXPORT void sg_getBagPosition(
|
||||
const SSG_planeCalibPara poseCalibPara,
|
||||
std::vector<SSG_peakRgnInfo>& objOps);
|
||||
|
||||
SG_APISHARED_EXPORT void sg_getBagPositionAndOrientation(
|
||||
SVzNLXYZRGBDLaserLine* laser3DPoints,
|
||||
int lineNum,
|
||||
//std::vector<SSG_lineFeature>& all_vLineFeatures,
|
||||
//std::vector<std::vector<int>>& noisePts,
|
||||
const SG_bagPositionParam algoParam,
|
||||
const SSG_planeCalibPara poseCalibPara,
|
||||
const SSG_hsvCmpParam colorCmpParam,
|
||||
const RGB rgbColorPattern, //用于区分袋子方向的颜色
|
||||
const double frontColorTemplate[RGN_HIST_SIZE],
|
||||
const double backColorTemplate[RGN_HIST_SIZE],
|
||||
std::vector<SSG_peakOrienRgnInfo>& objOps,
|
||||
#if OUTPUT_DEBUG
|
||||
std::vector<std::vector< SVzNL3DPosition>>& bagPositionCloudPts,
|
||||
#endif
|
||||
int* errCode);
|
||||
|
||||
SG_APISHARED_EXPORT void sg_sideBagPosition(
|
||||
SVzNL3DLaserLine* laser3DPoints,
|
||||
int lineNum,
|
||||
|
||||
@ -75,14 +75,6 @@ SG_APISHARED_EXPORT void sg_getLineLocalPeaks(
|
||||
std::vector< SSG_basicFeature1D>& localMax,
|
||||
std::vector< SSG_basicFeature1D>& localMin);
|
||||
|
||||
SG_APISHARED_EXPORT void sg_getFlatLineLocalPeaks_vector(
|
||||
std::vector<SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
const double scaleWin,
|
||||
const double minPkHeighth,
|
||||
const double holeR,
|
||||
std::vector< SSG_basicFeature1D>& localMax);
|
||||
|
||||
SG_APISHARED_EXPORT void sg_getLineDownJumps(
|
||||
SVzNL3DPosition* lineData,
|
||||
int dataSize,
|
||||
@ -96,13 +88,6 @@ SG_APISHARED_EXPORT void sg_getFeatureGrowingTrees(
|
||||
std::vector<SSG_featureTree>& trees,
|
||||
SSG_treeGrowParam growParam);
|
||||
|
||||
SG_APISHARED_EXPORT void sg_lineFeaturesGrowing(
|
||||
int lineIdx,
|
||||
bool isLastLine,
|
||||
std::vector<SSG_basicFeature1D>& features,
|
||||
std::vector<SSG_featureTree>& trees,
|
||||
SSG_treeGrowParam growParam);
|
||||
|
||||
//对ending进行生长
|
||||
SG_APISHARED_EXPORT void sg_getEndingGrowingTrees(
|
||||
std::vector<SSG_2DValueI>& lineEndings,
|
||||
@ -227,10 +212,6 @@ SG_APISHARED_EXPORT void sg_getLineRigthAngleFeature(
|
||||
SG_APISHARED_EXPORT SVzNL3DRangeD sg_getScanDataROI(
|
||||
SVzNL3DLaserLine* laser3DPoints,
|
||||
int lineNum);
|
||||
//¼ÆËãɨÃèROI: vecotr¸ñʽ
|
||||
SG_APISHARED_EXPORT SVzNL3DRangeD sg_getScanDataROI_vector(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines
|
||||
);
|
||||
|
||||
//XY平面直线拟合
|
||||
SG_APISHARED_EXPORT void lineFitting(
|
||||
@ -284,17 +265,3 @@ SG_APISHARED_EXPORT void lineDataRT(
|
||||
SVzNL3DLaserLine* a_line,
|
||||
const double* camPoseR,
|
||||
double groundH);
|
||||
SG_APISHARED_EXPORT void lineDataRT_vector(
|
||||
std::vector< SVzNL3DPosition>& a_line,
|
||||
const double* camPoseR,
|
||||
double groundH);
|
||||
SG_APISHARED_EXPORT void lineDataRT_RGBD(
|
||||
SVzNLXYZRGBDLaserLine* a_line,
|
||||
const double* camPoseR,
|
||||
double groundH);
|
||||
|
||||
SG_APISHARED_EXPORT void sg_pointClustering(
|
||||
std::vector< SVzNL3DPosition>& pts,
|
||||
double clusterDist,
|
||||
std::vector<std::vector< SVzNL3DPosition>>& objClusters //result
|
||||
);
|
||||
@ -2,7 +2,6 @@
|
||||
|
||||
#include <VZNL_Types.h>
|
||||
#include <vector>
|
||||
#include <SG_errCode.h>
|
||||
|
||||
#define PI 3.141592654
|
||||
|
||||
@ -56,18 +55,6 @@ typedef struct
|
||||
double bottom;
|
||||
}SSG_ROIRectD;
|
||||
|
||||
struct HSV {
|
||||
double h; // 色相 (0-360)
|
||||
double s; // 饱和度 (0-1)
|
||||
double v; // 亮度 (0-1)
|
||||
};
|
||||
|
||||
struct RGB {
|
||||
int r;
|
||||
int g;
|
||||
int b;
|
||||
};
|
||||
|
||||
#define LINE_FEATURE_NUM 16
|
||||
#define LINE_FEATURE_UNDEF 0
|
||||
#define LINE_FEATURE_L_JUMP_H2L 1
|
||||
@ -324,15 +311,6 @@ typedef struct
|
||||
SSG_6DOF centerPos;
|
||||
}SSG_peakRgnInfo;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int pkRgnIdx;
|
||||
SVzNLSizeD objSize;
|
||||
SVzNL2DPoint pos2D;
|
||||
SSG_6DOF centerPos;
|
||||
int orienFlag; //0-未知，1 - 正面， 2-反面
|
||||
}SSG_peakOrienRgnInfo;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
SVzNL3DPoint sPt;
|
||||
|
||||
@ -64,66 +64,6 @@ SVzNL3DRangeD sg_getScanDataROI(
|
||||
return roi;
|
||||
}
|
||||
|
||||
SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||||
{
|
||||
SVzNL3DRangeD roi;
|
||||
roi.xRange = { 0, -1 };
|
||||
roi.yRange = { 0, -1 };
|
||||
roi.zRange = { 0, -1 };
|
||||
|
||||
int lineNum = scanLines.size();
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int nPositionCnt = scanLines[line].size();
|
||||
for (int i = 0; i < nPositionCnt; i++)
|
||||
{
|
||||
SVzNL3DPosition* pt3D = &scanLines[line][i];
|
||||
if (pt3D->pt3D.z < 1e-4)
|
||||
continue;
|
||||
|
||||
if (roi.xRange.max < roi.xRange.min)
|
||||
{
|
||||
roi.xRange.min = pt3D->pt3D.x;
|
||||
roi.xRange.max = pt3D->pt3D.x;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (roi.xRange.min > pt3D->pt3D.x)
|
||||
roi.xRange.min = pt3D->pt3D.x;
|
||||
if (roi.xRange.max < pt3D->pt3D.x)
|
||||
roi.xRange.max = pt3D->pt3D.x;
|
||||
}
|
||||
//y
|
||||
if (roi.yRange.max < roi.yRange.min)
|
||||
{
|
||||
roi.yRange.min = pt3D->pt3D.y;
|
||||
roi.yRange.max = pt3D->pt3D.y;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (roi.yRange.min > pt3D->pt3D.y)
|
||||
roi.yRange.min = pt3D->pt3D.y;
|
||||
if (roi.yRange.max < pt3D->pt3D.y)
|
||||
roi.yRange.max = pt3D->pt3D.y;
|
||||
}
|
||||
//z
|
||||
if (roi.zRange.max < roi.zRange.min)
|
||||
{
|
||||
roi.zRange.min = pt3D->pt3D.z;
|
||||
roi.zRange.max = pt3D->pt3D.z;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (roi.zRange.min > pt3D->pt3D.z)
|
||||
roi.zRange.min = pt3D->pt3D.z;
|
||||
if (roi.zRange.max < pt3D->pt3D.z)
|
||||
roi.zRange.max = pt3D->pt3D.z;
|
||||
}
|
||||
}
|
||||
}
|
||||
return roi;
|
||||
}
|
||||
|
||||
void lineFitting(std::vector< SVzNL3DPoint>& inliers, double* _k, double* _b)
|
||||
{
|
||||
//最小二乘拟合直线参数
|
||||
@ -936,10 +876,9 @@ SSG_planeCalibPara sg_getPlaneCalibPara(
|
||||
zHist[histPos] ++;
|
||||
}
|
||||
}
|
||||
//以厘米为单位进行累加
|
||||
std::vector<int> zSumHist;
|
||||
zSumHist.resize(zHistSize);
|
||||
bool isSame = true;
|
||||
//以厘米为单位进行累加
|
||||
for (int i = 0; i < zHistSize; i++)
|
||||
{
|
||||
int sumValue = 0;
|
||||
@ -949,17 +888,6 @@ SSG_planeCalibPara sg_getPlaneCalibPara(
|
||||
sumValue += zHist[j];
|
||||
}
|
||||
zSumHist[i] = sumValue;
|
||||
if (i > 0)
|
||||
{
|
||||
if (sumValue != zSumHist[i - 1])
|
||||
isSame = false;
|
||||
}
|
||||
}
|
||||
if(true == isSame)
|
||||
{
|
||||
//不进行累加（如果累加，累加值相等）
|
||||
for (int i = 0; i < zHistSize; i++)
|
||||
zSumHist[i] = zHist[i];
|
||||
}
|
||||
|
||||
//寻找极值
|
||||
@ -1008,8 +936,6 @@ SSG_planeCalibPara sg_getPlaneCalibPara(
|
||||
pkTop.push_back({pre_i, pre_data});
|
||||
_state = 2;
|
||||
}
|
||||
else if(i == (zHistSize-1))
|
||||
pkTop.push_back({ i, curr_data });
|
||||
break;
|
||||
case 2: //下降
|
||||
if (z_diff > 0) // 上升
|
||||
@ -1019,12 +945,6 @@ SSG_planeCalibPara sg_getPlaneCalibPara(
|
||||
pkBtm.push_back({ pre_i, pre_data });
|
||||
_state = 1;
|
||||
}
|
||||
else if (i == (zHistSize - 1))
|
||||
{
|
||||
int pkBtmIdx = pkBtm.size();
|
||||
pkBtmBackIndexing[i] = pkBtmIdx;
|
||||
pkBtm.push_back({ i, curr_data });
|
||||
}
|
||||
break;
|
||||
default:
|
||||
_state = 0;
|
||||
@ -1458,41 +1378,3 @@ void lineDataRT(SVzNL3DLaserLine* a_line, const double* camPoseR, double groundH
|
||||
}
|
||||
return;
|
||||
}
|
||||
void lineDataRT_vector(std::vector< SVzNL3DPosition>& a_line, const double* camPoseR, double groundH)
|
||||
{
|
||||
for (int i = 0; i < a_line.size(); i++)
|
||||
{
|
||||
SVzNL3DPoint a_pt = a_line[i].pt3D;
|
||||
if (a_pt.z < 1e-4)
|
||||
continue;
|
||||
double x = a_pt.x * camPoseR[0] + a_pt.y * camPoseR[1] + a_pt.z * camPoseR[2];
|
||||
double y = a_pt.x * camPoseR[3] + a_pt.y * camPoseR[4] + a_pt.z * camPoseR[5];
|
||||
double z = a_pt.x * camPoseR[6] + a_pt.y * camPoseR[7] + a_pt.z * camPoseR[8];
|
||||
if ((groundH > 0) && (z > groundH)) //去除地面
|
||||
z = 0;
|
||||
a_pt.x = x;
|
||||
a_pt.y = y;
|
||||
a_pt.z = z;
|
||||
a_line[i].pt3D = a_pt;
|
||||
}
|
||||
return;
|
||||
}
|
||||
void lineDataRT_RGBD(SVzNLXYZRGBDLaserLine* a_line, const double* camPoseR, double groundH)
|
||||
{
|
||||
for (int i = 0; i < a_line->nPointCnt; i++)
|
||||
{
|
||||
SVzNLPointXYZRGBA a_pt = a_line->p3DPoint[i];
|
||||
if (a_pt.z < 1e-4)
|
||||
continue;
|
||||
double x = a_pt.x * camPoseR[0] + a_pt.y * camPoseR[1] + a_pt.z * camPoseR[2];
|
||||
double y = a_pt.x * camPoseR[3] + a_pt.y * camPoseR[4] + a_pt.z * camPoseR[5];
|
||||
double z = a_pt.x * camPoseR[6] + a_pt.y * camPoseR[7] + a_pt.z * camPoseR[8];
|
||||
if ((groundH > 0) && (z > groundH)) //去除地面
|
||||
z = 0;
|
||||
a_pt.x = x;
|
||||
a_pt.y = y;
|
||||
a_pt.z = z;
|
||||
a_line->p3DPoint[i] = a_pt;
|
||||
}
|
||||
return;
|
||||
}
|
||||
@ -1,67 +0,0 @@
|
||||
#include "SG_baseDataType.h"
|
||||
#include "SG_baseAlgo_Export.h"
|
||||
#include <vector>
|
||||
#include <corecrt_math_defines.h>
|
||||
#include <cmath>
|
||||
|
||||
void _seedClustering(
|
||||
std::vector< SVzNL3DPosition>& a_cluster,
|
||||
std::vector< SVzNL3DPosition>& pts,
|
||||
double clusterDist)
|
||||
{
|
||||
int i = 0;
|
||||
while (1)
|
||||
{
|
||||
if (i >= a_cluster.size())
|
||||
break;
|
||||
|
||||
SVzNL3DPosition a_seed = a_cluster[i];
|
||||
for (int i = 0, i_max = pts.size(); i < i_max; i++)
|
||||
{
|
||||
if (pts[i].nPointIdx < 0)
|
||||
continue;
|
||||
double dist = sqrt(pow(a_seed.pt3D.x - pts[i].pt3D.x, 2) + pow(a_seed.pt3D.y - pts[i].pt3D.y, 2));
|
||||
if (dist < clusterDist)
|
||||
{
|
||||
a_cluster.push_back(pts[i]);
|
||||
pts[i].nPointIdx = -1;
|
||||
}
|
||||
}
|
||||
i++;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void sg_pointClustering(
|
||||
std::vector< SVzNL3DPosition>& pts,
|
||||
double clusterDist,
|
||||
std::vector<std::vector< SVzNL3DPosition>>& objClusters //result
|
||||
)
|
||||
{
|
||||
int ptSize = pts.size();
|
||||
if (ptSize == 0)
|
||||
return;
|
||||
while(pts.size() > 0)
|
||||
{
|
||||
SVzNL3DPosition a_pt = pts[0];
|
||||
|
||||
//新建一个cluster
|
||||
std::vector< SVzNL3DPosition> a_cluster;
|
||||
a_cluster.push_back(a_pt);
|
||||
pts[0].nPointIdx = -1; //防止重复被计算
|
||||
_seedClustering(
|
||||
a_cluster,
|
||||
pts,
|
||||
clusterDist);
|
||||
objClusters.push_back(a_cluster); //保存一个聚类
|
||||
//将pts中处理过的点去除
|
||||
int m_max = pts.size();
|
||||
for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
|
||||
{
|
||||
if(pts[m].nPointIdx < 0)
|
||||
pts.erase(pts.begin() + m);
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
;
|
||||
@ -407,54 +407,6 @@ void sg_getFeatureGrowingTrees(
|
||||
}
|
||||
}
|
||||
|
||||
void sg_lineFeaturesGrowing(
|
||||
int lineIdx,
|
||||
bool isLastLine,
|
||||
std::vector<SSG_basicFeature1D>& features,
|
||||
std::vector<SSG_featureTree>& trees,
|
||||
SSG_treeGrowParam growParam)
|
||||
{
|
||||
if (features.size() > 0)
|
||||
{
|
||||
for (int j = 0, j_max = features.size(); j < j_max; j++)
|
||||
{
|
||||
SSG_basicFeature1D& a_feature = features[j];
|
||||
if (a_feature.jumpPos2D.x== 207)
|
||||
int kkk = 1;
|
||||
bool isMatched = _featureGrowing(a_feature, lineIdx, trees, growParam);
|
||||
if (false == isMatched)
|
||||
{
|
||||
//新的生长树
|
||||
SSG_featureTree a_newTree;
|
||||
a_newTree.treeNodes.push_back(a_feature);
|
||||
a_newTree.treeState = TREE_STATE_ALIVE;
|
||||
a_newTree.treeType = a_feature.featureType;
|
||||
a_newTree.sLineIdx = lineIdx;
|
||||
a_newTree.eLineIdx = lineIdx;
|
||||
trees.push_back(a_newTree);
|
||||
}
|
||||
}
|
||||
}
|
||||
//检查停止生长的树,加速。
|
||||
//将生长节点为1的生长树移除
|
||||
int m_max = trees.size();
|
||||
for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
|
||||
{
|
||||
if (TREE_STATE_ALIVE == trees[m].treeState)
|
||||
{
|
||||
int line_diff = abs(lineIdx - trees[m].treeNodes.back().jumpPos2D.x);
|
||||
if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) ||
|
||||
(true == isLastLine))
|
||||
{
|
||||
trees[m].treeState = TREE_STATE_DEAD;
|
||||
bool isValid = _invalidateVSlopeTrees(trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);
|
||||
if (false == isValid)
|
||||
trees.erase(trees.begin() + m);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void sg_getEndingGrowingTrees(
|
||||
std::vector<SSG_2DValueI>& lineEndings,
|
||||
SVzNL3DLaserLine* laser3DPoints,
|
||||
|
||||
@ -149,10 +149,7 @@ if (i == 370)
|
||||
}
|
||||
|
||||
//滤除离群点：z跳变门限方法
|
||||
void sg_lineDataRemoveOutlier_changeOriginData(
|
||||
SVzNL3DPosition* lineData,
|
||||
int dataSize,
|
||||
SSG_outlierFilterParam filterParam)
|
||||
void sg_lineDataRemoveOutlier_changeOriginData(SVzNL3DPosition* lineData, int dataSize, SSG_outlierFilterParam filterParam)
|
||||
{
|
||||
std::vector< SSG_RUN> continueRuns;
|
||||
SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx
|
||||
@ -1542,804 +1539,6 @@ void sg_getLineLocalPeaks(
|
||||
return;
|
||||
}
|
||||
|
||||
#if 0
|
||||
typedef struct
|
||||
{
|
||||
double holeLen;
|
||||
double holeCenterY;
|
||||
double holeCenterX;
|
||||
int holeSIdx;
|
||||
int holeEIdx;
|
||||
}_HoleInfo;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double maxZ;
|
||||
double minZ;
|
||||
double pk_y;
|
||||
double pk_x;
|
||||
int pkIdx;
|
||||
}_Feature;
|
||||
//对调平后的直线寻找突起。采用分段法寻找。
|
||||
void sg_getFlatLineLocalPeaks_vector(
|
||||
std::vector<SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
const double scaleWin,
|
||||
const double minPkHeighth,
|
||||
const double holeR,
|
||||
std::vector< SSG_basicFeature1D>& localMax)
|
||||
{
|
||||
int dataSize = lineData.size();
|
||||
if (dataSize < 2)
|
||||
return;
|
||||
|
||||
double maxZ = 0;
|
||||
double subScale = scaleWin / 3.0;
|
||||
std::vector<_Feature> subFeatures;
|
||||
int maxIdx = -1;
|
||||
int minIdx = -1;
|
||||
bool startVld = false;
|
||||
double startY = 0;
|
||||
int preVldPt = 0;
|
||||
int holePtNum = 0;
|
||||
_HoleInfo a_hole = { 0.0, 0.0, 0.0, -1, -1 };
|
||||
for (int i = 0; i < dataSize; i++)
|
||||
{
|
||||
lineData[i].nPointIdx = 0;
|
||||
if ((lineIdx == 1219) && (i == 2722))
|
||||
int kkk = 1;
|
||||
|
||||
if (lineData[i].pt3D.z < 1e-4)
|
||||
{
|
||||
if (true == startVld)
|
||||
holePtNum++;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (maxZ < lineData[i].pt3D.z)
|
||||
maxZ = lineData[i].pt3D.z;
|
||||
|
||||
if (false == startVld)
|
||||
{
|
||||
startVld = true;
|
||||
startY = lineData[i].pt3D.y;
|
||||
}
|
||||
//检查孔洞
|
||||
if ((holePtNum > 0) && (preVldPt >= 0))
|
||||
{
|
||||
double holeLen = lineData[i].pt3D.y - lineData[preVldPt].pt3D.y;
|
||||
if (holeLen > holeR / 3)
|
||||
{
|
||||
_HoleInfo currHole;
|
||||
currHole.holeLen = holeLen;
|
||||
currHole.holeCenterY = (lineData[i].pt3D.y + lineData[preVldPt].pt3D.y) / 2;
|
||||
currHole.holeCenterX = (lineData[i].pt3D.x + lineData[preVldPt].pt3D.x) / 2;
|
||||
currHole.holeSIdx = preVldPt;
|
||||
currHole.holeEIdx = i;
|
||||
|
||||
if (a_hole.holeSIdx >= 0)
|
||||
{
|
||||
//检查孔洞是否可以合并，如果不能合并，则取大的孔洞
|
||||
double hole_dist = currHole.holeCenterY - a_hole.holeCenterY;
|
||||
if (hole_dist < holeR * 2)
|
||||
{
|
||||
a_hole.holeEIdx = currHole.holeEIdx;
|
||||
a_hole.holeLen = lineData[a_hole.holeEIdx].pt3D.y - lineData[a_hole.holeSIdx].pt3D.y;
|
||||
a_hole.holeCenterY = (lineData[a_hole.holeEIdx].pt3D.y + lineData[a_hole.holeSIdx].pt3D.y) / 2;
|
||||
}
|
||||
else if (a_hole.holeLen < currHole.holeLen)
|
||||
a_hole = currHole;
|
||||
|
||||
}
|
||||
else
|
||||
a_hole = currHole;
|
||||
}
|
||||
//重新检查孔洞
|
||||
holePtNum = 0;
|
||||
}
|
||||
|
||||
double diffY = lineData[i].pt3D.y - startY;
|
||||
if (diffY >= subScale) //分段检查
|
||||
{
|
||||
if (a_hole.holeSIdx >= 0)
|
||||
{
|
||||
_Feature a_subFeature;
|
||||
a_subFeature.minZ = -1;
|
||||
a_subFeature.maxZ = -1;
|
||||
a_subFeature.pk_y = a_hole.holeCenterY;
|
||||
a_subFeature.pk_x = a_hole.holeCenterX;
|
||||
a_subFeature.pkIdx = (a_hole.holeSIdx + a_hole.holeEIdx) / 2;
|
||||
subFeatures.push_back(a_subFeature);
|
||||
}
|
||||
else if (maxIdx >= 0)
|
||||
{
|
||||
double depth = lineData[maxIdx].pt3D.z - lineData[minIdx].pt3D.z;
|
||||
if (depth > minPkHeighth / 2)
|
||||
{
|
||||
_Feature a_subFeature;
|
||||
a_subFeature.minZ = lineData[minIdx].pt3D.z;
|
||||
a_subFeature.maxZ = lineData[maxIdx].pt3D.z;
|
||||
a_subFeature.pk_x = lineData[maxIdx].pt3D.x;
|
||||
a_subFeature.pk_y = lineData[maxIdx].pt3D.y;
|
||||
a_subFeature.pkIdx = maxIdx;
|
||||
subFeatures.push_back(a_subFeature);
|
||||
}
|
||||
}
|
||||
//分段重新开始
|
||||
maxIdx = -1;
|
||||
minIdx = -1;
|
||||
startY = lineData[i].pt3D.y;
|
||||
a_hole = { 0.0, 0.0, 0.0, -1, -1 };
|
||||
}
|
||||
|
||||
if (maxIdx < 0)
|
||||
maxIdx = i;
|
||||
else
|
||||
{
|
||||
if (lineData[maxIdx].pt3D.z < lineData[i].pt3D.z)
|
||||
maxIdx = i;
|
||||
}
|
||||
if (minIdx < 0)
|
||||
minIdx = i;
|
||||
else
|
||||
{
|
||||
if (lineData[minIdx].pt3D.z > lineData[i].pt3D.z)
|
||||
minIdx = i;
|
||||
}
|
||||
|
||||
//处理最后一个数据
|
||||
if (i == dataSize - 1)
|
||||
{
|
||||
if (a_hole.holeSIdx >= 0)
|
||||
{
|
||||
_Feature a_subFeature;
|
||||
a_subFeature.minZ = -1;
|
||||
a_subFeature.maxZ = -1;
|
||||
a_subFeature.pk_x = a_hole.holeCenterX;
|
||||
a_subFeature.pk_y = a_hole.holeCenterY;
|
||||
a_subFeature.pkIdx = (a_hole.holeSIdx + a_hole.holeEIdx) / 2;
|
||||
subFeatures.push_back(a_subFeature);
|
||||
}
|
||||
else if (maxIdx >= 0)
|
||||
{
|
||||
_Feature a_subFeature;
|
||||
a_subFeature.minZ = lineData[minIdx].pt3D.z;
|
||||
a_subFeature.maxZ = lineData[maxIdx].pt3D.z;
|
||||
a_subFeature.pk_x = lineData[maxIdx].pt3D.x;
|
||||
a_subFeature.pk_y = lineData[maxIdx].pt3D.y;
|
||||
a_subFeature.pkIdx = maxIdx;
|
||||
subFeatures.push_back(a_subFeature);
|
||||
}
|
||||
}
|
||||
preVldPt = i;
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0, i_max = subFeatures.size(); i < i_max; i++)
|
||||
{
|
||||
_Feature* currFeature = &subFeatures[i];
|
||||
if (currFeature->pkIdx < 0)
|
||||
continue;
|
||||
if (currFeature->maxZ < 0) // 孔洞
|
||||
{
|
||||
bool isCombined = false;
|
||||
if (i < i_max - 1) //向后合并
|
||||
{
|
||||
_Feature* nxtFeature = &subFeatures[i + 1];
|
||||
//检查孔洞是否可以合并，如果不能合并，则取大的孔洞
|
||||
double hole_dist = nxtFeature->pk_y - currFeature->pk_y;
|
||||
if (hole_dist < holeR*2) //合并
|
||||
{
|
||||
if (nxtFeature->maxZ < 0)
|
||||
{
|
||||
nxtFeature->pkIdx = (currFeature->pkIdx + nxtFeature->pkIdx) / 2;
|
||||
nxtFeature->pk_x = (currFeature->pk_x + nxtFeature->pk_x) / 2;
|
||||
nxtFeature->pk_y = (currFeature->pk_y + nxtFeature->pk_y) / 2;
|
||||
}
|
||||
currFeature->pkIdx = -1; //合并标记
|
||||
isCombined = true;
|
||||
}
|
||||
}
|
||||
if (false == isCombined)
|
||||
{
|
||||
//生成特征
|
||||
SSG_basicFeature1D a_feature;
|
||||
a_feature.featureType = LINE_FEATURE_PEAK_TOP;
|
||||
a_feature.jumpPos2D = { lineIdx, currFeature->pkIdx };
|
||||
a_feature.jumpPos.x = currFeature->pk_x;
|
||||
a_feature.jumpPos.y = currFeature->pk_y;
|
||||
a_feature.jumpPos.z = maxZ;
|
||||
localMax.push_back(a_feature);
|
||||
lineData[currFeature->pkIdx].nPointIdx = LINE_FEATURE_PEAK_TOP; //nPointIdx被转义使用
|
||||
lineData[currFeature->pkIdx].pt3D = a_feature.jumpPos;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
bool isBigger_1 = true; //比前一个大
|
||||
bool isBigger_2 = true; //比后一个大
|
||||
double minZ = currFeature->minZ;
|
||||
|
||||
if (i > 0)
|
||||
{
|
||||
_Feature* preFeature = &subFeatures[i - 1];
|
||||
if (preFeature->maxZ < 0)
|
||||
{
|
||||
if(preFeature->pkIdx >= 0)
|
||||
isBigger_1 = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (currFeature->minZ > preFeature->minZ)
|
||||
minZ = preFeature->minZ;
|
||||
if (currFeature->maxZ < preFeature->maxZ)
|
||||
isBigger_1 = false;
|
||||
}
|
||||
}
|
||||
if (i < i_max - 1)
|
||||
{
|
||||
_Feature* nxtFeature = &subFeatures[i + 1];
|
||||
if (nxtFeature->maxZ < 0)
|
||||
{
|
||||
double hole_dist = nxtFeature->pk_y - currFeature->pk_y;
|
||||
if (hole_dist >= holeR * 2)
|
||||
isBigger_2 = false;
|
||||
else
|
||||
nxtFeature->pkIdx = -1; //后面的孔被合并
|
||||
}
|
||||
else
|
||||
{
|
||||
if (currFeature->minZ > nxtFeature->minZ)
|
||||
minZ = nxtFeature->minZ;
|
||||
if (currFeature->maxZ < nxtFeature->maxZ)
|
||||
isBigger_2 = false;
|
||||
}
|
||||
}
|
||||
if ((true == isBigger_1) && (true == isBigger_2))
|
||||
{
|
||||
double z_diff = currFeature->maxZ - minZ;
|
||||
if (z_diff > minPkHeighth) //合格的突起
|
||||
{
|
||||
SSG_basicFeature1D a_feature;
|
||||
a_feature.featureType = LINE_FEATURE_PEAK_TOP;
|
||||
a_feature.jumpPos2D = { lineIdx, currFeature->pkIdx };
|
||||
a_feature.jumpPos = lineData[currFeature->pkIdx].pt3D;
|
||||
localMax.push_back(a_feature);
|
||||
lineData[currFeature->pkIdx].nPointIdx = LINE_FEATURE_PEAK_TOP; //nPointIdx被转义使用
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
#else
|
||||
//对调平后的直线寻找突起。
|
||||
//根据斜率寻找：寻找跳变、斜坡、Gap，进行配对
|
||||
typedef struct
|
||||
{
|
||||
int type;
|
||||
int ptIdxS;
|
||||
int ptIdxE;
|
||||
int sIdx;
|
||||
int eIdx;
|
||||
SVzNL3DPoint start;
|
||||
SVzNL3DPoint end;
|
||||
double delta_y;
|
||||
double delta_z;
|
||||
double k;
|
||||
}_FeatureInfo;
|
||||
|
||||
int _findSegMaxZPt(
|
||||
std::vector<SVzNL3DPosition>& lineData,
|
||||
int searchStart,
|
||||
bool searchToHead,
|
||||
double searchWin,
|
||||
double* meanZ,
|
||||
bool* foundJump)
|
||||
{
|
||||
int dataSize = lineData.size();
|
||||
double start_y = lineData[searchStart].pt3D.y;
|
||||
double start_z = lineData[searchStart].pt3D.z;
|
||||
if (start_z < 1e-4)
|
||||
return -1;
|
||||
|
||||
double max_z = start_z;
|
||||
int max_idx = searchStart;
|
||||
double sum_z = max_z;
|
||||
int sum_num = 1;
|
||||
int step = searchToHead == true ? -1 : 1;
|
||||
int idx = searchStart;
|
||||
while (1)
|
||||
{
|
||||
idx += step;
|
||||
if ((idx <= 0) || (idx >= dataSize))
|
||||
break;
|
||||
|
||||
if (lineData[idx].nPointIdx == LINE_FEATURE_NUM)
|
||||
{
|
||||
*foundJump = true;
|
||||
break;
|
||||
}
|
||||
double pt_y = lineData[idx].pt3D.y;
|
||||
double pt_z = lineData[idx].pt3D.z;
|
||||
if (pt_z > 1e-4)
|
||||
{
|
||||
double dist = abs(pt_y - start_y);
|
||||
if (dist > searchWin)
|
||||
{
|
||||
*meanZ = sum_z / sum_num;
|
||||
return max_idx;
|
||||
}
|
||||
else
|
||||
{
|
||||
sum_z += pt_z;
|
||||
sum_num++;
|
||||
if (max_z < pt_z)
|
||||
{
|
||||
max_z = pt_z;
|
||||
max_idx = idx;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
void sg_getFlatLineLocalPeaks_vector(
|
||||
std::vector<SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
const double scaleWin,
|
||||
const double minPkHeighth,
|
||||
const double holeR,
|
||||
std::vector< SSG_basicFeature1D>& localMax)
|
||||
{
|
||||
int dataSize = lineData.size();
|
||||
if (dataSize < 2)
|
||||
return;
|
||||
|
||||
//加速，进行抽样
|
||||
//抽样的点中去除空点
|
||||
double slopeZDeltaTH = 0.2; //slope的z变化门限，大于此门限为有限slope
|
||||
int dwnSample = 1;
|
||||
int sampleNum = dataSize / dwnSample;
|
||||
std::vector<SVzNL3DPosition> sampleData;
|
||||
for (int i = 0; i < sampleNum; i++)
|
||||
{
|
||||
SVzNL3DPosition a_sample = lineData[i * dwnSample];
|
||||
a_sample.nPointIdx = i * dwnSample;
|
||||
if(a_sample.pt3D.z > 1e-4)
|
||||
sampleData.push_back(a_sample);
|
||||
}
|
||||
|
||||
sampleNum = sampleData.size();
|
||||
std::vector< _FeatureInfo> allSubFeatures;
|
||||
allSubFeatures.resize(sampleNum);
|
||||
for (int i = 0; i < sampleNum; i++) //初始化
|
||||
allSubFeatures[i].type = LINE_FEATURE_UNDEF;
|
||||
|
||||
int segEnd = 0;
|
||||
for (int i = 0; i < sampleNum-1; i++)
|
||||
{
|
||||
SVzNL3DPosition* samplePtr = &sampleData[i];
|
||||
if ((lineIdx == 0) && (i == 2290))
|
||||
int kkk = 1;
|
||||
//检查相邻两点是否为跳变或gap
|
||||
SVzNL3DPosition* nxtSamplePtr = &sampleData[i+1];
|
||||
//double d_delta = sqrt(pow(nxtSamplePtr->pt3D.y - samplePtr->pt3D.y, 2) + pow(nxtSamplePtr->pt3D.z - samplePtr->pt3D.z, 2));
|
||||
double y_delta = abs(nxtSamplePtr->pt3D.y - samplePtr->pt3D.y);
|
||||
double z_delta = nxtSamplePtr->pt3D.z - samplePtr->pt3D.z;
|
||||
if ((y_delta > holeR / 2) || (abs(z_delta) > minPkHeighth))
|
||||
{
|
||||
_FeatureInfo a_subFeature;
|
||||
a_subFeature.delta_y = y_delta;
|
||||
a_subFeature.delta_z = z_delta;
|
||||
a_subFeature.k = z_delta / y_delta;
|
||||
a_subFeature.ptIdxS = i;
|
||||
a_subFeature.ptIdxE = i+1;
|
||||
a_subFeature.sIdx = sampleData[i].nPointIdx;
|
||||
a_subFeature.eIdx = sampleData[i + 1].nPointIdx;
|
||||
a_subFeature.start = samplePtr->pt3D;
|
||||
a_subFeature.end = nxtSamplePtr->pt3D;
|
||||
if(z_delta > 0)
|
||||
a_subFeature.type = LINE_FEATURE_L_JUMP_L2H;
|
||||
else
|
||||
a_subFeature.type = LINE_FEATURE_L_JUMP_H2L;
|
||||
allSubFeatures[i] = a_subFeature;
|
||||
segEnd = i + 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
//寻找尺度端点
|
||||
for (int j = segEnd; j < sampleNum; j++)
|
||||
{
|
||||
SVzNL3DPosition* postSamplePtr = &sampleData[j];
|
||||
double deltaY = abs(postSamplePtr->pt3D.y - samplePtr->pt3D.y);
|
||||
if (deltaY >= scaleWin)
|
||||
{
|
||||
double deltaZ = postSamplePtr->pt3D.z - samplePtr->pt3D.z;
|
||||
if (abs(deltaZ) > slopeZDeltaTH)
|
||||
int kkk = 1;
|
||||
_FeatureInfo a_subFeature;
|
||||
a_subFeature.delta_y = deltaY;
|
||||
a_subFeature.delta_z = deltaZ;
|
||||
a_subFeature.k = deltaZ / deltaY;
|
||||
a_subFeature.ptIdxS = i;
|
||||
a_subFeature.ptIdxE = j;
|
||||
a_subFeature.sIdx = sampleData[i].nPointIdx;
|
||||
a_subFeature.eIdx = sampleData[j].nPointIdx;
|
||||
a_subFeature.start = samplePtr->pt3D;
|
||||
a_subFeature.end = postSamplePtr->pt3D;
|
||||
if ( (abs(a_subFeature.k) > 0.5) && (abs(deltaZ) > slopeZDeltaTH))
|
||||
{
|
||||
if (deltaZ > 0)
|
||||
a_subFeature.type = LINE_FEATURE_L_SLOPE_L2H;
|
||||
else
|
||||
a_subFeature.type = LINE_FEATURE_L_SLOPE_H2L;
|
||||
}
|
||||
else
|
||||
a_subFeature.type = LINE_FEATURE_UNDEF;
|
||||
allSubFeatures[i] = a_subFeature;
|
||||
segEnd = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//处理JUMP, 将相邻的LINE_FEATURE_L_SLOPE_L2H无效
|
||||
for (int i = 0; i < sampleNum; i++)
|
||||
{
|
||||
if (allSubFeatures[i].type == LINE_FEATURE_L_JUMP_L2H)
|
||||
{
|
||||
//将相邻的LINE_FEATURE_L_SLOPE_L2H无效
|
||||
for (int j = i + 1; j < sampleNum; j++)
|
||||
{
|
||||
if (allSubFeatures[j].type == LINE_FEATURE_L_SLOPE_L2H)
|
||||
allSubFeatures[j].type = LINE_FEATURE_UNDEF;
|
||||
else
|
||||
break;
|
||||
}
|
||||
}
|
||||
else if (allSubFeatures[i].type == LINE_FEATURE_L_JUMP_H2L)
|
||||
{
|
||||
//将相邻的LINE_FEATURE_L_SLOPE_L2H无效
|
||||
for (int j = i - 1; j >= 0; j--)
|
||||
{
|
||||
if (allSubFeatures[j].type == LINE_FEATURE_L_SLOPE_H2L)
|
||||
allSubFeatures[j].type = LINE_FEATURE_UNDEF;
|
||||
else
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
#if 0
|
||||
std::vector< _FeatureInfo> subFeatures;
|
||||
for (int i = 0; i < sampleNum; i++)
|
||||
{
|
||||
if (allSubFeatures[i].type != LINE_FEATURE_UNDEF)
|
||||
subFeatures.push_back(allSubFeatures[i]);
|
||||
}
|
||||
_FeatureInfo a_nullFeature;
|
||||
memset(&a_nullFeature, 0, sizeof(_FeatureInfo));
|
||||
subFeatures.push_back(a_nullFeature); //在结尾补上一个空，序列处理时会正确处理最后一个
|
||||
int subfeatureSize = subFeatures.size();
|
||||
#endif
|
||||
//将合格的subFeature挑选出来
|
||||
int pre_type = LINE_FEATURE_UNDEF;
|
||||
_FeatureInfo a_vldFeature;
|
||||
std::vector< _FeatureInfo> vldFeatures;
|
||||
for (int i = 0; i < sampleNum; i++)
|
||||
{
|
||||
if (pre_type == LINE_FEATURE_UNDEF)
|
||||
{
|
||||
if ( (allSubFeatures[i].type == LINE_FEATURE_L_JUMP_H2L) ||
|
||||
(allSubFeatures[i].type == LINE_FEATURE_L_JUMP_L2H))
|
||||
{
|
||||
vldFeatures.push_back(allSubFeatures[i]);
|
||||
pre_type = LINE_FEATURE_UNDEF;
|
||||
}
|
||||
else
|
||||
{
|
||||
a_vldFeature = allSubFeatures[i];
|
||||
pre_type = allSubFeatures[i].type;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (allSubFeatures[i].type == LINE_FEATURE_UNDEF)
|
||||
{
|
||||
vldFeatures.push_back(a_vldFeature);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (((a_vldFeature.k < 0) && (allSubFeatures[i].k < 0)) ||
|
||||
((a_vldFeature.k > 0) && (allSubFeatures[i].k > 0)))
|
||||
{
|
||||
if (abs(a_vldFeature.k) < abs(allSubFeatures[i].k))
|
||||
a_vldFeature = allSubFeatures[i];
|
||||
}
|
||||
else
|
||||
{
|
||||
vldFeatures.push_back(a_vldFeature);
|
||||
a_vldFeature = allSubFeatures[i];
|
||||
}
|
||||
}
|
||||
pre_type = allSubFeatures[i].type;
|
||||
}
|
||||
}
|
||||
|
||||
//配对，生成特征
|
||||
//优先对Jump进行配对
|
||||
std::vector<SSG_basicFeature1D> featureBuffer;
|
||||
featureBuffer.resize(vldFeatures.size());
|
||||
for (int i = 0; i < vldFeatures.size(); i++) //初始化
|
||||
featureBuffer[i].featureType = LINE_FEATURE_UNDEF;
|
||||
for (int i = 0, i_max = vldFeatures.size(); i < i_max; i++)
|
||||
{
|
||||
if (vldFeatures[i].type == LINE_FEATURE_L_JUMP_H2L)
|
||||
{
|
||||
//判断z的幅度
|
||||
if (abs(vldFeatures[i].delta_z) < minPkHeighth) //没有起伏，可以匹配后面的JUMP
|
||||
{
|
||||
//生成特征
|
||||
SSG_basicFeature1D a_feature;
|
||||
a_feature.featureType = LINE_FEATURE_PEAK_TOP;
|
||||
int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2;
|
||||
a_feature.jumpPos2D = { lineIdx, centerIdx };
|
||||
a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2;
|
||||
a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2;
|
||||
a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2;
|
||||
int start = vldFeatures[i].sIdx;
|
||||
int end = vldFeatures[i].eIdx;
|
||||
//尝试匹配后面的JUMP
|
||||
if (i < i_max - 1)
|
||||
{
|
||||
if ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_H2L) ||
|
||||
(vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_L2H))
|
||||
{
|
||||
//判断距离
|
||||
double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y);
|
||||
if (dist < holeR) //合并
|
||||
{
|
||||
centerIdx = (vldFeatures[i].sIdx + vldFeatures[i+1].eIdx) / 2;
|
||||
a_feature.jumpPos2D = { lineIdx, centerIdx };
|
||||
a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i+1].end.x) / 2;
|
||||
a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i+1].end.y) / 2;
|
||||
a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i+1].end.z) / 2;
|
||||
vldFeatures[i + 1].type = LINE_FEATURE_UNDEF;
|
||||
end = vldFeatures[i+1].eIdx;
|
||||
}
|
||||
}
|
||||
}
|
||||
featureBuffer[i] = a_feature;
|
||||
lineData[start].nPointIdx = LINE_FEATURE_NUM; //起点和终点作标识。使用LINE_FEATURE_NUM作标识码
|
||||
lineData[end].nPointIdx = LINE_FEATURE_NUM;
|
||||
}
|
||||
else //有明显下降，只能匹配后面没有起伏的JUMP
|
||||
{
|
||||
//生成特征
|
||||
SSG_basicFeature1D a_feature;
|
||||
a_feature.featureType = LINE_FEATURE_PEAK_TOP;
|
||||
int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2;
|
||||
a_feature.jumpPos2D = { lineIdx, centerIdx };
|
||||
a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2;
|
||||
a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2;
|
||||
a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2;
|
||||
int start = vldFeatures[i].sIdx;
|
||||
int end = vldFeatures[i].eIdx;
|
||||
if (i < i_max - 1)
|
||||
{
|
||||
if ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_H2L) ||
|
||||
(vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_L2H))
|
||||
{
|
||||
if (abs(vldFeatures[i + 1].delta_z) < minPkHeighth) //只匹配没有起伏的
|
||||
{
|
||||
//判断距离
|
||||
double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y);
|
||||
if (dist < holeR) //合并
|
||||
{
|
||||
centerIdx = (vldFeatures[i].sIdx + vldFeatures[i + 1].eIdx) / 2;
|
||||
a_feature.jumpPos2D = { lineIdx, centerIdx };
|
||||
a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i + 1].end.x) / 2;
|
||||
a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i + 1].end.y) / 2;
|
||||
a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i + 1].end.z) / 2;
|
||||
vldFeatures[i + 1].type = LINE_FEATURE_UNDEF;
|
||||
end = vldFeatures[i + 1].eIdx;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
featureBuffer[i] = a_feature;
|
||||
lineData[start].nPointIdx = LINE_FEATURE_NUM; //起点和终点作标识。使用LINE_FEATURE_NUM作标识码
|
||||
lineData[end].nPointIdx = LINE_FEATURE_NUM;
|
||||
}
|
||||
}
|
||||
else if (vldFeatures[i].type == LINE_FEATURE_L_JUMP_L2H)
|
||||
{
|
||||
//判断z的幅度
|
||||
if (abs(vldFeatures[i].delta_z) < minPkHeighth) //没有起伏，可以匹配后面的JUMP
|
||||
{
|
||||
//直接匹配后面的JUMP
|
||||
//生成特征
|
||||
SSG_basicFeature1D a_feature;
|
||||
a_feature.featureType = LINE_FEATURE_PEAK_TOP;
|
||||
int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2;
|
||||
a_feature.jumpPos2D = { lineIdx, centerIdx };
|
||||
a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2;
|
||||
a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2;
|
||||
a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2;
|
||||
int start = vldFeatures[i].sIdx;
|
||||
int end = vldFeatures[i].eIdx;
|
||||
//尝试匹配后面的JUMP
|
||||
if (i < i_max - 1)
|
||||
{
|
||||
if ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_H2L) ||
|
||||
(vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_L2H))
|
||||
{
|
||||
//判断距离
|
||||
double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y);
|
||||
if (dist < holeR) //合并
|
||||
{
|
||||
centerIdx = (vldFeatures[i].sIdx + vldFeatures[i + 1].eIdx) / 2;
|
||||
a_feature.jumpPos2D = { lineIdx, centerIdx };
|
||||
a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i + 1].end.x) / 2;
|
||||
a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i + 1].end.y) / 2;
|
||||
a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i + 1].end.z) / 2;
|
||||
vldFeatures[i + 1].type = LINE_FEATURE_UNDEF;
|
||||
end = vldFeatures[i + 1].eIdx;
|
||||
}
|
||||
}
|
||||
}
|
||||
featureBuffer[i] = a_feature;
|
||||
lineData[start].nPointIdx = LINE_FEATURE_NUM; //起点和终点作标识。使用LINE_FEATURE_NUM作标识码
|
||||
lineData[end].nPointIdx = LINE_FEATURE_NUM;
|
||||
}
|
||||
else //有起伏的，可以匹配后面的下降JUMP和没有起伏的JUMP和SLOPE
|
||||
{
|
||||
//生成特征
|
||||
SSG_basicFeature1D a_feature;
|
||||
a_feature.featureType = LINE_FEATURE_PEAK_TOP;
|
||||
int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2;
|
||||
a_feature.jumpPos2D = { lineIdx, centerIdx };
|
||||
a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2;
|
||||
a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2;
|
||||
a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2;
|
||||
int start = vldFeatures[i].sIdx;
|
||||
int end = vldFeatures[i].eIdx;
|
||||
if (i < i_max - 1)
|
||||
{
|
||||
if ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_H2L) ||
|
||||
((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_L2H) && (abs(vldFeatures[i+1].delta_z) < minPkHeighth)) ||
|
||||
(vldFeatures[i + 1].type == LINE_FEATURE_L_SLOPE_H2L))
|
||||
{
|
||||
//判断距离
|
||||
double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y);
|
||||
if (dist < holeR) //合并
|
||||
{
|
||||
centerIdx = (vldFeatures[i].sIdx + vldFeatures[i + 1].eIdx) / 2;
|
||||
a_feature.jumpPos2D = { lineIdx, centerIdx };
|
||||
a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i + 1].end.x) / 2;
|
||||
a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i + 1].end.y) / 2;
|
||||
a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i + 1].end.z) / 2;
|
||||
vldFeatures[i + 1].type = LINE_FEATURE_UNDEF;
|
||||
end = vldFeatures[i + 1].eIdx;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
featureBuffer[i] = a_feature;
|
||||
lineData[start].nPointIdx = LINE_FEATURE_NUM; //起点和终点作标识。使用LINE_FEATURE_NUM作标识码
|
||||
lineData[end].nPointIdx = LINE_FEATURE_NUM;
|
||||
}
|
||||
}
|
||||
}
|
||||
//对SLOPE进行配对
|
||||
for (int i = 0, i_max = vldFeatures.size(); i < i_max; i++)
|
||||
{
|
||||
if (vldFeatures[i].type == LINE_FEATURE_L_SLOPE_L2H)
|
||||
{
|
||||
if (i < i_max - 1)
|
||||
{
|
||||
double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y);
|
||||
if ((dist < holeR) && (vldFeatures[i + 1].type == LINE_FEATURE_L_SLOPE_H2L))
|
||||
{
|
||||
//生成特征
|
||||
SSG_basicFeature1D a_feature;
|
||||
a_feature.featureType = LINE_FEATURE_PEAK_TOP;
|
||||
int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i + 1].eIdx) / 2;
|
||||
a_feature.jumpPos2D = { lineIdx, centerIdx };
|
||||
a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i + 1].end.x) / 2;
|
||||
a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i + 1].end.y) / 2;
|
||||
a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i + 1].end.z) / 2;
|
||||
vldFeatures[i + 1].type = LINE_FEATURE_UNDEF;
|
||||
featureBuffer[i] = a_feature;
|
||||
}
|
||||
else
|
||||
{
|
||||
SSG_basicFeature1D a_feature;
|
||||
a_feature.featureType = LINE_FEATURE_PEAK_TOP;
|
||||
#if 0
|
||||
int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2;
|
||||
a_feature.jumpPos2D = { lineIdx, centerIdx };
|
||||
a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2;
|
||||
a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2;
|
||||
a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2;
|
||||
#endif
|
||||
//在HoleR范围内寻找Peak点
|
||||
double mean_z = -1;
|
||||
bool foundJump = false;
|
||||
int pkIdx = _findSegMaxZPt(lineData, vldFeatures[i].eIdx, false, holeR*1.5, &mean_z, &foundJump);
|
||||
//反向寻找是否有Jump
|
||||
double meanZ_inv = -1;
|
||||
bool foundJump_inv = false;
|
||||
int pkIdx_inv = _findSegMaxZPt(lineData, vldFeatures[i].sIdx, true, holeR * 1.5, &meanZ_inv, &foundJump_inv);
|
||||
if ( (pkIdx >= 0)&&(false == foundJump_inv))
|
||||
{
|
||||
double mean_depth = mean_z - vldFeatures[i].start.z;
|
||||
if (pkIdx_inv >= 0)
|
||||
mean_depth = mean_z - meanZ_inv;
|
||||
if (mean_depth >= slopeZDeltaTH/2)
|
||||
{
|
||||
a_feature.jumpPos2D = { lineIdx, pkIdx };
|
||||
a_feature.jumpPos = lineData[pkIdx].pt3D;
|
||||
featureBuffer[i] = a_feature;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (vldFeatures[i].type == LINE_FEATURE_L_SLOPE_H2L)
|
||||
{
|
||||
//在HoleR范围内寻找Peak点
|
||||
SSG_basicFeature1D a_feature;
|
||||
a_feature.featureType = LINE_FEATURE_PEAK_TOP;
|
||||
#if 0
|
||||
int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2;
|
||||
a_feature.jumpPos2D = { lineIdx, centerIdx };
|
||||
a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2;
|
||||
a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2;
|
||||
a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2;
|
||||
#endif
|
||||
//在HoleR范围内寻找Peak点
|
||||
double mean_z = -1;
|
||||
bool foundJump = false;
|
||||
int pkIdx = _findSegMaxZPt(lineData, vldFeatures[i].sIdx, true, holeR*1.5, &mean_z, &foundJump);
|
||||
//反向寻找是否有Jump
|
||||
double meanZ_inv = -1;
|
||||
bool foundJump_inv = false;
|
||||
int pkIdx_inv = _findSegMaxZPt(lineData, vldFeatures[i].eIdx, false, holeR * 1.5, &meanZ_inv, &foundJump_inv);
|
||||
if ((pkIdx >= 0) && (false == foundJump_inv))
|
||||
{
|
||||
double mean_depth = mean_z - vldFeatures[i].start.z;
|
||||
if (pkIdx_inv >= 0)
|
||||
mean_depth = mean_z - meanZ_inv;
|
||||
if (mean_depth >= slopeZDeltaTH/2)
|
||||
{
|
||||
a_feature.jumpPos2D = { lineIdx, pkIdx };
|
||||
a_feature.jumpPos = lineData[pkIdx].pt3D;
|
||||
featureBuffer[i] = a_feature;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
for (int i = 0, i_max = vldFeatures.size(); i < i_max; i++)
|
||||
{
|
||||
if (featureBuffer[i].featureType != LINE_FEATURE_UNDEF)
|
||||
{
|
||||
localMax.push_back(featureBuffer[i]);
|
||||
if (lineData[featureBuffer[i].jumpPos2D.y].pt3D.z < 1e-4)
|
||||
lineData[featureBuffer[i].jumpPos2D.y].pt3D = featureBuffer[i].jumpPos;
|
||||
lineData[featureBuffer[i].jumpPos2D.y].nPointIdx = LINE_FEATURE_PEAK_TOP; //nPointIdx被转义使用
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
||||
/// <summary>
|
||||
/// 提取激光线上的下跳变点（Z值变大的跳变）
|
||||
///
|
||||
|
||||
@ -1,811 +0,0 @@
|
||||
#include <vector>
|
||||
#include "SG_baseDataType.h"
|
||||
#include "SG_baseAlgo_Export.h"
|
||||
#include "WD_QRcode3Ddetection_Export.h"
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <limits>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int id;
|
||||
double dist;
|
||||
double angle;
|
||||
}_linkInfo;
|
||||
typedef struct
|
||||
{
|
||||
int objId;
|
||||
SVzNL3DPoint ptPos;
|
||||
_linkInfo link[8]; //R, RT, T, LT, L, LB, B, RB
|
||||
}NodeOctLink;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
SSG_ROIRectD roi;
|
||||
int* qrCode;
|
||||
}DMCodeInfo;
|
||||
|
||||
//将ply格式的数据恢复成扫描行的数据形式，从而方面按行进行处理
|
||||
void wd_getScanLines(
|
||||
std::vector<SVzNL3DPoint>& scanData,
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
int scan_rows)
|
||||
{
|
||||
std::vector<SVzNL3DPosition> a_line;
|
||||
for (int i = 0, i_max = scanData.size(); i < i_max; i++)
|
||||
{
|
||||
int idx = i % scan_rows;
|
||||
if (0 == idx)
|
||||
{
|
||||
//新的一行
|
||||
if (a_line.size() > 0)
|
||||
{
|
||||
scanLines.push_back(a_line);
|
||||
a_line.clear();
|
||||
}
|
||||
}
|
||||
SVzNL3DPoint a_pt = scanData[i];
|
||||
SVzNL3DPosition a_idxPt;
|
||||
a_idxPt.nPointIdx = 0;
|
||||
a_idxPt.pt3D = a_pt;
|
||||
a_line.push_back(a_idxPt);
|
||||
}
|
||||
if(a_line.size() > 0)
|
||||
scanLines.push_back(a_line);
|
||||
return;
|
||||
}
|
||||
|
||||
SSG_planeCalibPara wd_getBaseCalibPara(
|
||||
SVzNL3DLaserLine* laser3DPoints,
|
||||
int lineNum)
|
||||
{
|
||||
return sg_getPlaneCalibPara(laser3DPoints, lineNum);
|
||||
}
|
||||
|
||||
void wd_lineDataR(std::vector< SVzNL3DPosition>& a_line,
|
||||
const double* camPoseR,
|
||||
double groundH)
|
||||
{
|
||||
lineDataRT_vector(a_line, camPoseR, groundH);
|
||||
}
|
||||
|
||||
SVzNL3DPoint _getObjCenter(SSG_featureTree& a_tree)
|
||||
{
|
||||
SVzNL3DPoint centerPt = { 0, 0, 0 };
|
||||
int nodeNum = 0;
|
||||
for (int i = 0; i < a_tree.treeNodes.size(); i++)
|
||||
{
|
||||
centerPt.x += a_tree.treeNodes[i].jumpPos.x;
|
||||
centerPt.y += a_tree.treeNodes[i].jumpPos.y;
|
||||
centerPt.z += a_tree.treeNodes[i].jumpPos.z;
|
||||
nodeNum++;
|
||||
}
|
||||
if (nodeNum > 0)
|
||||
{
|
||||
centerPt.x = centerPt.x / nodeNum;
|
||||
centerPt.y = centerPt.y / nodeNum;
|
||||
centerPt.z = centerPt.z / nodeNum;
|
||||
}
|
||||
return centerPt;
|
||||
}
|
||||
|
||||
_linkInfo compareBestLinl(_linkInfo link_1, _linkInfo link_2, double bestDist)
|
||||
{
|
||||
double dist_diff1 = abs(link_1.dist - bestDist);
|
||||
double dist_diff2 = abs(link_2.dist - bestDist);
|
||||
if (dist_diff1 < dist_diff2)
|
||||
return link_1;
|
||||
else
|
||||
return link_2;
|
||||
}
|
||||
|
||||
//建立Node间的上下左右的Link关系
|
||||
void _createNodeLinks(
|
||||
std::vector< SVzNL3DPosition>& a_cluster,
|
||||
std::vector< NodeOctLink>& clusterNodeLinks,
|
||||
double row_space,
|
||||
double col_space
|
||||
)
|
||||
{
|
||||
int dirInvTbl[8] = { 4, 5, 6, 7, 0, 1, 2, 3 };
|
||||
double diaDist = sqrt(pow(row_space, 2) + pow(col_space, 2));
|
||||
int nodeSize = a_cluster.size();
|
||||
for (int i = 0; i < nodeSize; i++)
|
||||
{
|
||||
SVzNL3DPosition& a_node = a_cluster[i];
|
||||
NodeOctLink& a_nodeLink = clusterNodeLinks[i];
|
||||
_linkInfo link[8];
|
||||
for (int j = 0; j < 8; j++)
|
||||
{
|
||||
link[j].id = -1;
|
||||
link[j].dist = 0.0;
|
||||
link[j].angle = 0.0;
|
||||
}
|
||||
for (int j = i+1; j < nodeSize; j++)
|
||||
{
|
||||
SVzNL3DPosition& chk_node = a_cluster[j];
|
||||
NodeOctLink& chk_nodeLink = clusterNodeLinks[j];
|
||||
if (chk_node.nPointIdx < 0)
|
||||
continue;
|
||||
|
||||
double dist = sqrt(pow(a_node.pt3D.x - chk_node.pt3D.x, 2) + pow(a_node.pt3D.y - chk_node.pt3D.y, 2));
|
||||
if (dist < diaDist * 2) //粗过滤
|
||||
{
|
||||
double angle = atan2(a_node.pt3D.y - chk_node.pt3D.y, chk_node.pt3D.x - a_node.pt3D.x); //图像坐标系y方向与欧氏坐标系相么
|
||||
angle = angle * 180.0 / PI; //转为角度
|
||||
if (angle < 0)
|
||||
angle += 360; //转成0-360度
|
||||
|
||||
_linkInfo a_link = { j, dist, angle };
|
||||
if ((angle > 345) || (angle < 15)) //R:0
|
||||
{
|
||||
if (dist < col_space * 1.5)
|
||||
{
|
||||
if (link[0].id < 0)
|
||||
link[0] = a_link;
|
||||
else
|
||||
link[0] = compareBestLinl(link[0], a_link, col_space);
|
||||
}
|
||||
}
|
||||
else if ((angle > 30) && (angle < 60)) //RT:1
|
||||
{
|
||||
if (dist < diaDist * 1.5)
|
||||
{
|
||||
if (link[1].id < 0)
|
||||
link[1] = a_link;
|
||||
else
|
||||
link[1] = compareBestLinl(link[1], a_link, diaDist);
|
||||
}
|
||||
}
|
||||
else if ((angle > 75) && (angle < 105)) //T:2
|
||||
{
|
||||
if (dist < row_space * 1.5)
|
||||
{
|
||||
if (link[2].id < 0)
|
||||
link[2] = a_link;
|
||||
else
|
||||
link[2] = compareBestLinl(link[2], a_link, row_space);
|
||||
}
|
||||
}
|
||||
else if ((angle > 120) && (angle < 150)) //LT:3
|
||||
{
|
||||
if (dist < diaDist * 1.5)
|
||||
{
|
||||
if (link[3].id < 0)
|
||||
link[3] = a_link;
|
||||
else
|
||||
link[3] = compareBestLinl(link[3], a_link, diaDist);
|
||||
}
|
||||
}
|
||||
else if ((angle > 165) && (angle < 195)) //L:4
|
||||
{
|
||||
if (dist < col_space * 1.5)
|
||||
{
|
||||
if (link[4].id < 0)
|
||||
link[4] = a_link;
|
||||
else
|
||||
link[4] = compareBestLinl(link[4], a_link, col_space);
|
||||
}
|
||||
}
|
||||
else if ((angle > 210) && (angle < 240)) //LB:5
|
||||
{
|
||||
if (dist < diaDist * 1.5)
|
||||
{
|
||||
if (link[5].id < 0)
|
||||
link[5] = a_link;
|
||||
else
|
||||
link[5] = compareBestLinl(link[5], a_link, diaDist);
|
||||
}
|
||||
}
|
||||
else if ((angle > 255) && (angle < 285)) //B:6
|
||||
{
|
||||
if (dist < row_space * 1.5)
|
||||
{
|
||||
if (link[6].id < 0)
|
||||
link[6] = a_link;
|
||||
else
|
||||
link[6] = compareBestLinl(link[6], a_link, row_space);
|
||||
}
|
||||
}
|
||||
else if ((angle > 300) && (angle < 330)) //RB:7
|
||||
{
|
||||
if (dist < diaDist * 1.5)
|
||||
{
|
||||
if (link[7].id < 0)
|
||||
link[7] = a_link;
|
||||
else
|
||||
link[7] = compareBestLinl(link[7], a_link, diaDist);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int j = 0; j < 8; j++)
|
||||
{
|
||||
if (link[j].id >= 0)
|
||||
{
|
||||
a_nodeLink.link[j] = link[j];
|
||||
int linkId = link[j].id;
|
||||
int dir_inv = dirInvTbl[j];
|
||||
if (clusterNodeLinks[linkId].link[dir_inv].id < 0)
|
||||
{
|
||||
clusterNodeLinks[linkId].link[dir_inv].id = a_nodeLink.objId;
|
||||
clusterNodeLinks[linkId].link[dir_inv].dist = link[j].dist;
|
||||
clusterNodeLinks[linkId].link[dir_inv].angle = link[j].angle + 180;
|
||||
if (clusterNodeLinks[linkId].link[dir_inv].angle >= 360)
|
||||
clusterNodeLinks[linkId].link[dir_inv].angle = clusterNodeLinks[linkId].link[dir_inv].angle - 360;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
std::vector<int> _LTseedSearchRight(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
|
||||
{
|
||||
NodeOctLink a_seed = startNode;
|
||||
std::vector<int> result;
|
||||
result.push_back(a_seed.objId);
|
||||
while (1)
|
||||
{
|
||||
if (a_seed.link[0].id < 0)
|
||||
break;
|
||||
a_seed = clusterNodeLinks[a_seed.link[0].id];
|
||||
|
||||
if ((a_seed.link[1].id < 0) && (a_seed.link[2].id < 0) && (a_seed.link[3].id < 0))
|
||||
result.push_back(a_seed.objId);
|
||||
else
|
||||
break;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
std::vector<int> _LTseedSearchBottom(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
|
||||
{
|
||||
NodeOctLink a_seed = startNode;
|
||||
std::vector<int> result;
|
||||
result.push_back(a_seed.objId);
|
||||
while (1)
|
||||
{
|
||||
if (a_seed.link[6].id < 0)
|
||||
break;
|
||||
a_seed = clusterNodeLinks[a_seed.link[6].id];
|
||||
|
||||
if ((a_seed.link[3].id < 0) && (a_seed.link[4].id < 0) && (a_seed.link[5].id < 0))
|
||||
result.push_back(a_seed.objId);
|
||||
else
|
||||
break;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
int _foundDM_LTcorner(std::vector< NodeOctLink>& clusterNodeLinks, int rows, int cols)
|
||||
{
|
||||
for (int i = 0, i_max = clusterNodeLinks.size(); i < i_max; i++)
|
||||
{
|
||||
//寻找种子向右向下
|
||||
NodeOctLink a_seed = clusterNodeLinks[i];
|
||||
if ((a_seed.link[0].id >= 0) && (a_seed.link[6].id >= 0) &&
|
||||
(a_seed.link[1].id < 0) && (a_seed.link[2].id < 0) &&
|
||||
(a_seed.link[3].id < 0) && (a_seed.link[4].id < 0) &&
|
||||
(a_seed.link[5].id < 0)) //合格的种子
|
||||
{
|
||||
std::vector<int> R_result = _LTseedSearchRight(a_seed, clusterNodeLinks);
|
||||
std::vector<int> B_result = _LTseedSearchBottom(a_seed, clusterNodeLinks);
|
||||
if (((int)R_result.size() == cols) && ((int)B_result.size() == rows)) //找到LT角点
|
||||
return i;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
std::vector<int> _RTseedSearchLeft(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
|
||||
{
|
||||
NodeOctLink a_seed = startNode;
|
||||
std::vector<int> result;
|
||||
result.push_back(a_seed.objId);
|
||||
while (1)
|
||||
{
|
||||
if (a_seed.link[4].id < 0)
|
||||
break;
|
||||
a_seed = clusterNodeLinks[a_seed.link[4].id];
|
||||
|
||||
if ((a_seed.link[1].id < 0) && (a_seed.link[2].id < 0) && (a_seed.link[3].id < 0))
|
||||
result.push_back(a_seed.objId);
|
||||
else
|
||||
break;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
std::vector<int> _RTseedSearchBottom(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
|
||||
{
|
||||
NodeOctLink a_seed = startNode;
|
||||
std::vector<int> result;
|
||||
result.push_back(a_seed.objId);
|
||||
while (1)
|
||||
{
|
||||
if (a_seed.link[6].id < 0)
|
||||
break;
|
||||
a_seed = clusterNodeLinks[a_seed.link[6].id];
|
||||
|
||||
if ((a_seed.link[1].id < 0) && (a_seed.link[0].id < 0) && (a_seed.link[7].id < 0))
|
||||
result.push_back(a_seed.objId);
|
||||
else
|
||||
break;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
int _foundDM_RTcorner(std::vector< NodeOctLink>& clusterNodeLinks, int rows, int cols)
|
||||
{
|
||||
for (int i = 0, i_max = clusterNodeLinks.size(); i < i_max; i++)
|
||||
{
|
||||
//寻找种子向右向下
|
||||
NodeOctLink a_seed = clusterNodeLinks[i];
|
||||
if ((a_seed.link[4].id >= 0) && (a_seed.link[6].id >= 0) &&
|
||||
(a_seed.link[0].id < 0) && (a_seed.link[1].id < 0) &&
|
||||
(a_seed.link[2].id < 0) && (a_seed.link[3].id < 0) &&
|
||||
(a_seed.link[7].id < 0)) //合格的种子
|
||||
{
|
||||
std::vector<int> L_result = _RTseedSearchLeft(a_seed, clusterNodeLinks);
|
||||
std::vector<int> B_result = _RTseedSearchBottom(a_seed, clusterNodeLinks);
|
||||
if (((int)L_result.size() == cols) && ((int)B_result.size() == rows)) //找到LT角点
|
||||
return i;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
std::vector<int> _LBseedSearchRight(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
|
||||
{
|
||||
NodeOctLink a_seed = startNode;
|
||||
std::vector<int> result;
|
||||
result.push_back(a_seed.objId);
|
||||
while (1)
|
||||
{
|
||||
if (a_seed.link[0].id < 0)
|
||||
break;
|
||||
a_seed = clusterNodeLinks[a_seed.link[0].id];
|
||||
|
||||
if ((a_seed.link[5].id < 0) && (a_seed.link[6].id < 0) && (a_seed.link[7].id < 0))
|
||||
result.push_back(a_seed.objId);
|
||||
else
|
||||
break;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
std::vector<int> _LBseedSearchTop(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
|
||||
{
|
||||
NodeOctLink a_seed = startNode;
|
||||
std::vector<int> result;
|
||||
result.push_back(a_seed.objId);
|
||||
while (1)
|
||||
{
|
||||
if (a_seed.link[2].id < 0)
|
||||
break;
|
||||
a_seed = clusterNodeLinks[a_seed.link[2].id];
|
||||
|
||||
if ((a_seed.link[3].id < 0) && (a_seed.link[4].id < 0) && (a_seed.link[5].id < 0))
|
||||
result.push_back(a_seed.objId);
|
||||
else
|
||||
break;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
int _foundDM_LBcorner(std::vector< NodeOctLink>& clusterNodeLinks, int rows, int cols)
|
||||
{
|
||||
for (int i = 0, i_max = clusterNodeLinks.size(); i < i_max; i++)
|
||||
{
|
||||
//寻找种子向右向下
|
||||
NodeOctLink a_seed = clusterNodeLinks[i];
|
||||
if ((a_seed.link[0].id >= 0) && (a_seed.link[2].id >= 0) &&
|
||||
(a_seed.link[3].id < 0) && (a_seed.link[4].id < 0) &&
|
||||
(a_seed.link[5].id < 0) && (a_seed.link[6].id < 0) &&
|
||||
(a_seed.link[7].id < 0)) //合格的种子
|
||||
{
|
||||
std::vector<int> L_result = _LBseedSearchRight(a_seed, clusterNodeLinks);
|
||||
std::vector<int> B_result = _LBseedSearchTop(a_seed, clusterNodeLinks);
|
||||
if (((int)L_result.size() == cols) && ((int)B_result.size() == rows)) //找到LT角点
|
||||
return i;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
std::vector<int> _RBseedSearchLeft(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
|
||||
{
|
||||
NodeOctLink a_seed = startNode;
|
||||
std::vector<int> result;
|
||||
result.push_back(a_seed.objId);
|
||||
while (1)
|
||||
{
|
||||
if (a_seed.link[4].id < 0)
|
||||
break;
|
||||
a_seed = clusterNodeLinks[a_seed.link[4].id];
|
||||
|
||||
if ((a_seed.link[5].id < 0) && (a_seed.link[6].id < 0) && (a_seed.link[7].id < 0))
|
||||
result.push_back(a_seed.objId);
|
||||
else
|
||||
break;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
std::vector<int> _RBseedSearchTop(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
|
||||
{
|
||||
NodeOctLink a_seed = startNode;
|
||||
std::vector<int> result;
|
||||
result.push_back(a_seed.objId);
|
||||
while (1)
|
||||
{
|
||||
if (a_seed.link[2].id < 0)
|
||||
break;
|
||||
a_seed = clusterNodeLinks[a_seed.link[2].id];
|
||||
|
||||
if ((a_seed.link[1].id < 0) && (a_seed.link[0].id < 0) && (a_seed.link[7].id < 0))
|
||||
result.push_back(a_seed.objId);
|
||||
else
|
||||
break;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
int _foundDM_RBcorner(std::vector< NodeOctLink>& clusterNodeLinks, int rows, int cols)
|
||||
{
|
||||
for (int i = 0, i_max = clusterNodeLinks.size(); i < i_max; i++)
|
||||
{
|
||||
//寻找种子向右向下
|
||||
NodeOctLink a_seed = clusterNodeLinks[i];
|
||||
if ((a_seed.ptPos.x > 19.2) && (a_seed.ptPos.y > -31))
|
||||
int kkk = 1;
|
||||
if ((a_seed.link[2].id >= 0) && (a_seed.link[4].id >= 0) &&
|
||||
(a_seed.link[0].id < 0) && (a_seed.link[1].id < 0) &&
|
||||
(a_seed.link[5].id < 0) && (a_seed.link[6].id < 0) &&
|
||||
(a_seed.link[7].id < 0)) //合格的种子
|
||||
{
|
||||
std::vector<int> L_result = _RBseedSearchLeft(a_seed, clusterNodeLinks);
|
||||
std::vector<int> B_result = _RBseedSearchTop(a_seed, clusterNodeLinks);
|
||||
if (((int)L_result.size() == cols) && ((int)B_result.size() == rows)) //找到LT角点
|
||||
return i;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int idx;
|
||||
SVzNL2DPoint pt2D;
|
||||
}DMPos;
|
||||
|
||||
void _getDMPosLinks(std::vector< DMPos>& posLink, std::vector< NodeOctLink>& clusterNodeLinks)
|
||||
{
|
||||
int i = 0;
|
||||
while (1)
|
||||
{
|
||||
if (i >= posLink.size())
|
||||
break;
|
||||
DMPos a_seed = posLink[i];
|
||||
NodeOctLink& a_node = clusterNodeLinks[a_seed.idx];
|
||||
if (a_node.objId >= 0)
|
||||
{
|
||||
if (a_node.link[0].id >= 0)
|
||||
{
|
||||
DMPos new_seed;
|
||||
new_seed.idx = a_node.link[0].id;
|
||||
new_seed.pt2D = { a_seed.pt2D.x + 1, a_seed.pt2D.y };
|
||||
if (clusterNodeLinks[new_seed.idx].objId >= 0)
|
||||
{
|
||||
clusterNodeLinks[new_seed.idx].link[4].id = -1; //标记已经处理
|
||||
posLink.push_back(new_seed);
|
||||
}
|
||||
}
|
||||
if (a_node.link[1].id >= 0)
|
||||
{
|
||||
DMPos new_seed;
|
||||
new_seed.idx = a_node.link[1].id;
|
||||
new_seed.pt2D = { a_seed.pt2D.x + 1, a_seed.pt2D.y - 1 };
|
||||
if (clusterNodeLinks[new_seed.idx].objId >= 0)
|
||||
{
|
||||
clusterNodeLinks[new_seed.idx].link[5].id = -1; //标记已经处理
|
||||
posLink.push_back(new_seed);
|
||||
}
|
||||
}
|
||||
if (a_node.link[2].id >= 0)
|
||||
{
|
||||
DMPos new_seed;
|
||||
new_seed.idx = a_node.link[2].id;
|
||||
new_seed.pt2D = { a_seed.pt2D.x, a_seed.pt2D.y - 1 };
|
||||
if (clusterNodeLinks[new_seed.idx].objId >= 0)
|
||||
{
|
||||
clusterNodeLinks[new_seed.idx].link[6].id = -1; //标记已经处理
|
||||
posLink.push_back(new_seed);
|
||||
}
|
||||
}
|
||||
if (a_node.link[3].id >= 0)
|
||||
{
|
||||
DMPos new_seed;
|
||||
new_seed.idx = a_node.link[3].id;
|
||||
new_seed.pt2D = { a_seed.pt2D.x - 1, a_seed.pt2D.y - 1 };
|
||||
clusterNodeLinks[new_seed.idx].link[7].id = -1; //标记已经处理
|
||||
posLink.push_back(new_seed);
|
||||
}
|
||||
if (a_node.link[4].id >= 0)
|
||||
{
|
||||
DMPos new_seed;
|
||||
new_seed.idx = a_node.link[4].id;
|
||||
new_seed.pt2D = { a_seed.pt2D.x - 1, a_seed.pt2D.y };
|
||||
if (clusterNodeLinks[new_seed.idx].objId >= 0)
|
||||
{
|
||||
clusterNodeLinks[new_seed.idx].link[0].id = -1; //标记已经处理
|
||||
posLink.push_back(new_seed);
|
||||
}
|
||||
}
|
||||
if (a_node.link[5].id >= 0)
|
||||
{
|
||||
DMPos new_seed;
|
||||
new_seed.idx = a_node.link[5].id;
|
||||
new_seed.pt2D = { a_seed.pt2D.x - 1, a_seed.pt2D.y + 1 };
|
||||
if (clusterNodeLinks[new_seed.idx].objId >= 0)
|
||||
{
|
||||
clusterNodeLinks[new_seed.idx].link[1].id = -1; //标记已经处理
|
||||
posLink.push_back(new_seed);
|
||||
}
|
||||
}
|
||||
if (a_node.link[6].id >= 0)
|
||||
{
|
||||
DMPos new_seed;
|
||||
new_seed.idx = a_node.link[6].id;
|
||||
new_seed.pt2D = { a_seed.pt2D.x, a_seed.pt2D.y + 1 };
|
||||
if (clusterNodeLinks[new_seed.idx].objId >= 0)
|
||||
{
|
||||
clusterNodeLinks[new_seed.idx].link[2].id = -1; //标记已经处理
|
||||
posLink.push_back(new_seed);
|
||||
}
|
||||
}
|
||||
if (a_node.link[7].id >= 0)
|
||||
{
|
||||
DMPos new_seed;
|
||||
new_seed.idx = a_node.link[7].id;
|
||||
new_seed.pt2D = { a_seed.pt2D.x + 1, a_seed.pt2D.y + 1 };
|
||||
if (clusterNodeLinks[new_seed.idx].objId >= 0)
|
||||
{
|
||||
clusterNodeLinks[new_seed.idx].link[3].id = -1; //标记已经处理
|
||||
posLink.push_back(new_seed);
|
||||
}
|
||||
}
|
||||
clusterNodeLinks[a_seed.idx].objId = -1; //标记已经处理
|
||||
}
|
||||
i++;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
SSG_ROIRectD _getClusterROI(std::vector< SVzNL3DPosition>& a_cluster)
|
||||
{
|
||||
SSG_ROIRectD roi = {0,0,0,0};
|
||||
int nodeSize = a_cluster.size();
|
||||
if (nodeSize == 0)
|
||||
return roi;
|
||||
|
||||
roi = { a_cluster [0].pt3D.x, a_cluster[0].pt3D.x, a_cluster[0].pt3D.y, a_cluster[0].pt3D.y};
|
||||
for (int i = 1; i < nodeSize; i++)
|
||||
{
|
||||
roi.left = roi.left > a_cluster[i].pt3D.x ? a_cluster[i].pt3D.x : roi.left;
|
||||
roi.right = roi.right < a_cluster[i].pt3D.x ? a_cluster[i].pt3D.x : roi.right;
|
||||
roi.top = roi.top > a_cluster[i].pt3D.y ? a_cluster[i].pt3D.y : roi.top;
|
||||
roi.bottom = roi.bottom < a_cluster[i].pt3D.y ? a_cluster[i].pt3D.y : roi.bottom;
|
||||
}
|
||||
return roi;
|
||||
}
|
||||
|
||||
void wd_QRcode3Ddetection(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const WD_QRcodeParam qrcode_param,
|
||||
std::vector< SVzNL3DPosition>& outObjPoints,
|
||||
cv::Mat& dmCodeImg)
|
||||
{
|
||||
std::vector<std::vector< SSG_basicFeature1D>> lineZMaxPts;
|
||||
int lineNum = scanLines.size();
|
||||
|
||||
double scaleWin = qrcode_param.row_space / 5;
|
||||
double minPkHeighth = qrcode_param.pointHoleDepth / 2;
|
||||
double holeR = qrcode_param.pointHoleR;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
std::vector< SSG_basicFeature1D> a_lineMax;
|
||||
sg_getFlatLineLocalPeaks_vector(
|
||||
scanLines[line],
|
||||
line,
|
||||
scaleWin,
|
||||
minPkHeighth,
|
||||
holeR,
|
||||
a_lineMax);
|
||||
lineZMaxPts.push_back(a_lineMax);
|
||||
}
|
||||
//特征生长
|
||||
SSG_treeGrowParam growParam;
|
||||
growParam.yDeviation_max = holeR;//生长时相邻特征最大的Y偏差
|
||||
growParam.zDeviation_max = holeR; //生长时相邻特征最大的Z偏差
|
||||
growParam.maxLineSkipNum = 5; //生长时相邻特征的最大线间隔， -1时使用maxDkipDistance
|
||||
growParam.maxSkipDistance = -1; //若maxLineSkipNum为-1， 使用此参数.设为-1时，此参数无效
|
||||
growParam.minLTypeTreeLen = holeR/2; //生长树最少的节点数目。小于此数目的生长树被移除
|
||||
growParam.minVTypeTreeLen = holeR/2; //生长树最少的节点数目。小于此数目的生长树被移除
|
||||
|
||||
std::vector<SSG_featureTree> trees;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
std::vector< SSG_basicFeature1D>& a_lineMax = lineZMaxPts[line];
|
||||
if (a_lineMax.size() == 0)
|
||||
continue;
|
||||
|
||||
//对feature进行生长
|
||||
bool isLastLine = false;
|
||||
if (line == lineNum - 1)
|
||||
isLastLine = true;
|
||||
sg_lineFeaturesGrowing(
|
||||
line,
|
||||
isLastLine,
|
||||
a_lineMax,
|
||||
trees,
|
||||
growParam);
|
||||
}
|
||||
|
||||
//计算目标中心点，以此代表各个点
|
||||
std::vector< SVzNL3DPosition> objPoints;
|
||||
objPoints.resize(trees.size());
|
||||
for (int i = 0, i_max = trees.size(); i < i_max; i++)
|
||||
{
|
||||
SVzNL3DPosition a_obj;
|
||||
a_obj.nPointIdx = i;
|
||||
a_obj.pt3D = _getObjCenter(trees[i]);
|
||||
objPoints[i] = a_obj;
|
||||
}
|
||||
//将邻近的点合并
|
||||
int objPtSize = objPoints.size();
|
||||
for (int i = objPtSize - 1; i >= 0; i--)
|
||||
{
|
||||
SVzNL3DPosition a_pt = objPoints[i];
|
||||
bool isCombined = false;
|
||||
for (int j = i - 1; j >= 0; j--)
|
||||
{
|
||||
SVzNL3DPosition chk_pt = objPoints[j];
|
||||
double dist = sqrt(pow(a_pt.pt3D.x - chk_pt.pt3D.x, 2) + pow(a_pt.pt3D.y - chk_pt.pt3D.y, 2));
|
||||
if (dist < holeR * 2) //合并
|
||||
{
|
||||
objPoints[j].pt3D.x = (a_pt.pt3D.x + chk_pt.pt3D.x) / 2;
|
||||
objPoints[j].pt3D.y = (a_pt.pt3D.y + chk_pt.pt3D.y) / 2;
|
||||
objPoints[j].pt3D.z = (a_pt.pt3D.z + chk_pt.pt3D.z) / 2;
|
||||
isCombined = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (true == isCombined)
|
||||
objPoints.erase(objPoints.begin() + i);
|
||||
}
|
||||
for (int i = 0, i_max = objPoints.size(); i < i_max; i++)
|
||||
outObjPoints.push_back(objPoints[i]);
|
||||
|
||||
//聚类，分成一个个二维码区域
|
||||
double clusterDist = (qrcode_param.row_space + qrcode_param.col_space) * 2; //使用棋盘格距离
|
||||
std::vector<std::vector< SVzNL3DPosition>> objClusters;
|
||||
sg_pointClustering(
|
||||
objPoints,
|
||||
clusterDist,
|
||||
objClusters //result
|
||||
);
|
||||
|
||||
std::vector<DMCodeInfo> qrCodeResult;
|
||||
for (int i = 0, i_max = objClusters.size(); i < i_max; i++)
|
||||
{
|
||||
std::vector< SVzNL3DPosition>& a_cluster = objClusters[i];
|
||||
int clustNodeSize = a_cluster.size();
|
||||
//构建相邻关系
|
||||
std::vector< NodeOctLink> clusterNodeLinks;
|
||||
clusterNodeLinks.resize(clustNodeSize);
|
||||
for (int m = 0; m < clustNodeSize; m++) //初始化
|
||||
{
|
||||
clusterNodeLinks[m].objId = m;
|
||||
clusterNodeLinks[m].ptPos = a_cluster[m].pt3D;
|
||||
for (int j = 0; j < 8; j++)
|
||||
{
|
||||
clusterNodeLinks[m].link[j].id = -1;
|
||||
clusterNodeLinks[m].link[j].dist = 0.0;
|
||||
clusterNodeLinks[m].link[j].angle = 0.0;
|
||||
}
|
||||
}
|
||||
_createNodeLinks(a_cluster, clusterNodeLinks, qrcode_param.row_space, qrcode_param.col_space);
|
||||
//寻找DM二维码特征
|
||||
int cornerIdx = 0;
|
||||
int cornerNodeIdx = _foundDM_LTcorner(clusterNodeLinks, qrcode_param.rows, qrcode_param.cols);
|
||||
if (cornerNodeIdx < 0)
|
||||
{
|
||||
cornerIdx = 1;
|
||||
cornerNodeIdx = _foundDM_RTcorner(clusterNodeLinks, qrcode_param.rows, qrcode_param.cols);
|
||||
if (cornerNodeIdx < 0)
|
||||
{
|
||||
cornerIdx = 2;
|
||||
cornerNodeIdx = _foundDM_LBcorner(clusterNodeLinks, qrcode_param.rows, qrcode_param.cols);
|
||||
if (cornerNodeIdx < 0)
|
||||
{
|
||||
cornerIdx = 3;
|
||||
cornerNodeIdx = _foundDM_RBcorner(clusterNodeLinks, qrcode_param.rows, qrcode_param.cols);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (cornerNodeIdx >= 0)
|
||||
{
|
||||
//提取二维码编码值
|
||||
int rows = qrcode_param.rows;
|
||||
int cols = qrcode_param.cols;
|
||||
|
||||
DMPos startPos;
|
||||
startPos.idx = cornerNodeIdx;
|
||||
if (cornerIdx == 0) //LT
|
||||
startPos.pt2D = { 0,0 };
|
||||
else if (cornerIdx == 1) //RT
|
||||
startPos.pt2D = { 0,cols-1 };
|
||||
else if(cornerIdx == 2) //LB
|
||||
startPos.pt2D = { rows-1,0 };
|
||||
else //RB
|
||||
startPos.pt2D = { rows - 1, cols-1 };
|
||||
|
||||
std::vector< DMPos> posLinks;
|
||||
posLinks.push_back(startPos);
|
||||
_getDMPosLinks(posLinks, clusterNodeLinks);
|
||||
|
||||
int* codeBuff = (int*)malloc(sizeof(int) * rows * cols);
|
||||
memset(codeBuff, 0, sizeof(int) * qrcode_param.rows * qrcode_param.cols);
|
||||
for (int m = 0, m_max = posLinks.size(); m < m_max; m++)
|
||||
{
|
||||
int col = posLinks[m].pt2D.x;
|
||||
int row = posLinks[m].pt2D.y;
|
||||
codeBuff[row * cols + col] = 1;
|
||||
}
|
||||
//
|
||||
DMCodeInfo a_dmCode;
|
||||
a_dmCode.roi = _getClusterROI(a_cluster);
|
||||
a_dmCode.qrCode = codeBuff;
|
||||
qrCodeResult.push_back(a_dmCode);
|
||||
}
|
||||
}
|
||||
if (qrCodeResult.size() > 0)
|
||||
{
|
||||
SVzNL3DRangeD globalRoi = sg_getScanDataROI_vector(scanLines);
|
||||
//恢复二维码编码图像
|
||||
int dmNodeSize = 60;
|
||||
int dmLen = dmNodeSize * qrcode_param.cols;
|
||||
double scale = (qrCodeResult[0].roi.right - qrCodeResult[0].roi.left) / (double)dmLen;
|
||||
int skip = dmNodeSize;
|
||||
int img_cols = (int)((globalRoi.xRange.max - globalRoi.xRange.min) / scale);
|
||||
img_cols = ((img_cols + 3) / 4) * 4; //4对齐
|
||||
int img_rows = (int)((globalRoi.yRange.max - globalRoi.yRange.min) / scale);
|
||||
img_rows = ((img_rows + 3) / 4) * 4; //4对齐
|
||||
//加上空白边
|
||||
img_cols += skip * 2;
|
||||
img_rows += skip * 2;
|
||||
dmCodeImg = cv::Mat::ones(img_rows, img_cols, CV_8UC1);
|
||||
for (int i = 0; i < qrCodeResult.size(); i++)
|
||||
{
|
||||
double x = qrCodeResult[i].roi.left;
|
||||
double y = qrCodeResult[i].roi.top;
|
||||
SVzNL2DPoint cornerPos;
|
||||
cornerPos.x = (int)((x-globalRoi.xRange.min) / scale) + skip;
|
||||
cornerPos.y = (int)((y-globalRoi.yRange.min) / scale) + skip;
|
||||
for (int row = 0; row < qrcode_param.rows; row++)
|
||||
{
|
||||
for (int col = 0; col < qrcode_param.cols; col++)
|
||||
{
|
||||
SVzNL2DPoint nodePos;
|
||||
nodePos.x = cornerPos.x + col * dmNodeSize;
|
||||
nodePos.y = cornerPos.y + row * dmNodeSize;
|
||||
if (qrCodeResult[i].qrCode[row * qrcode_param.cols + col] > 0)
|
||||
{
|
||||
for (int m = -dmNodeSize / 2; m < dmNodeSize / 2; m++)
|
||||
{
|
||||
for (int n = -dmNodeSize / 2; n < dmNodeSize / 2; n++)
|
||||
dmCodeImg.at<uchar>(nodePos.y + m, nodePos.x + n) = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
free(qrCodeResult[i].qrCode);
|
||||
qrCodeResult[i].qrCode = NULL;
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
@ -1,42 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#if defined(SG_API_LIBRARY)
|
||||
# define SG_APISHARED_EXPORT __declspec(dllexport)
|
||||
#else
|
||||
# define SG_APISHARED_EXPORT __declspec(dllimport)
|
||||
#endif
|
||||
|
||||
#include "SG_baseDataType.h"
|
||||
#include <vector>
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int rows; //二维码行数
|
||||
int cols; //二维码列数
|
||||
double row_space; //行间距，单位mm
|
||||
double col_space; //列间距，单位mm
|
||||
double pointHoleDepth; //二维码点的深度，单位mm
|
||||
double pointHoleR; //二维码点的半径，单位mm
|
||||
}WD_QRcodeParam;
|
||||
|
||||
//将ply格式的数据恢复成扫描行的数据形式，从而方面按行进行处理
|
||||
SG_APISHARED_EXPORT void wd_getScanLines(
|
||||
std::vector<SVzNL3DPoint>& scanData,
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
int scan_rows);
|
||||
|
||||
SG_APISHARED_EXPORT SSG_planeCalibPara wd_getBaseCalibPara(
|
||||
SVzNL3DLaserLine* laser3DPoints,
|
||||
int lineNum);
|
||||
|
||||
SG_APISHARED_EXPORT void wd_lineDataR(
|
||||
std::vector< SVzNL3DPosition>& a_line,
|
||||
const double* camPoseR,
|
||||
double groundH);
|
||||
|
||||
SG_APISHARED_EXPORT void wd_QRcode3Ddetection(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const WD_QRcodeParam qrcode_param,
|
||||
std::vector< SVzNL3DPosition>& objPoints,
|
||||
cv::Mat& dmCodeImg);
|
||||
Loading…
x
Reference in New Issue
Block a user