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

planeLocalization

Browse Source 
version 1.0.0 : base version release to customer
This commit is contained in:
jerryzeng 2026-07-17 17:05:44 +08:00
parent 1f407d9fb9
commit a29a6c005c
6 changed files with 1742 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

158
HC_planeLocalization/HC_planeLocalization.vcxproj Normal file
View File

@ -0,0 +1,158 @@
<?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>17.0</VCProjectVersion>
<Keyword>Win32Proj</Keyword>
<ProjectGuid>{76989229-c695-43e6-af6a-ce8dd9980e16}</ProjectGuid>
<RootNamespace>HCplaneLocalization</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>v143</PlatformToolset>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
<ConfigurationType>DynamicLibrary</ConfigurationType>
<UseDebugLibraries>false</UseDebugLibraries>
<PlatformToolset>v143</PlatformToolset>
<WholeProgramOptimization>true</WholeProgramOptimization>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
<ConfigurationType>DynamicLibrary</ConfigurationType>
<UseDebugLibraries>true</UseDebugLibraries>
<PlatformToolset>v143</PlatformToolset>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
<ConfigurationType>DynamicLibrary</ConfigurationType>
<UseDebugLibraries>false</UseDebugLibraries>
<PlatformToolset>v143</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|x64'">
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
</PropertyGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
<ClCompile>
<WarningLevel>Level3</WarningLevel>
<SDLCheck>true</SDLCheck>
<PreprocessorDefinitions>WIN32;_DEBUG;HCPLANELOCALIZATION_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;HCPLANELOCALIZATION_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)'=='Debug|x64'">
<ClCompile>
<WarningLevel>Level3</WarningLevel>
<SDLCheck>true</SDLCheck>
<PreprocessorDefinitions>_CRT_SECURE_NO_WARNINGS;_DEBUG;HCPLANELOCALIZATION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<ConformanceMode>true</ConformanceMode>
<PrecompiledHeader>NotUsing</PrecompiledHeader>
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
</ClCompile>
<Link>
<SubSystem>Windows</SubSystem>
<GenerateDebugInformation>true</GenerateDebugInformation>
<EnableUAC>false</EnableUAC>
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
<AdditionalDependencies>opencv_world320d.lib;baseAlgorithm.lib;%(AdditionalDependencies)</AdditionalDependencies>
</Link>
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
<ClCompile>
<WarningLevel>Level3</WarningLevel>
<FunctionLevelLinking>true</FunctionLevelLinking>
<IntrinsicFunctions>true</IntrinsicFunctions>
<SDLCheck>true</SDLCheck>
<PreprocessorDefinitions>_CRT_SECURE_NO_WARNINGS;NDEBUG;HCPLANELOCALIZATION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<ConformanceMode>true</ConformanceMode>
<PrecompiledHeader>NotUsing</PrecompiledHeader>
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
</ClCompile>
<Link>
<SubSystem>Windows</SubSystem>
<GenerateDebugInformation>true</GenerateDebugInformation>
<EnableUAC>false</EnableUAC>
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
<AdditionalDependencies>opencv_world320.lib;baseAlgorithm.lib;%(AdditionalDependencies)</AdditionalDependencies>
</Link>
</ItemDefinitionGroup>
<ItemGroup>
<ClInclude Include="..\sourceCode\planeLocalization_Export.h" />
</ItemGroup>
<ItemGroup>
<ClCompile Include="..\sourceCode\planeLocalization.cpp" />
</ItemGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
<ImportGroup Label="ExtensionTargets">
</ImportGroup>
</Project>

650
HC_planeLocalization_test/HC_planeLocalization_test.cpp Normal file
View File

@ -0,0 +1,650 @@
// gasFillingPortPosition_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//
#include <iostream>
#include <fstream>
#include <vector>
#include <stdio.h>
#include <VZNL_Types.h>
#include "direct.h"
#include <string>
#include "planeLocalization_Export.h"
#include <opencv2/opencv.hpp>
#include <Windows.h>
#include <limits>
typedef struct
{
int r;
int g;
int b;
}SG_color;
typedef struct
{
int nPointIdx;
double x;
double y;
double z;
float r;
float g;
float b;
} SPointXYZRGB;
SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, const double matrix3d[9])
{
SVzNL3DPoint _r_pt;
_r_pt.x = pt3D.x * matrix3d[0] + pt3D.y * matrix3d[1] + pt3D.z * matrix3d[2];
_r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5];
_r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8];
return _r_pt;
}
void wdReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData)
{
std::ifstream inputFile(fileName);
std::string linedata;
if (inputFile.is_open() == false)
return;
std::vector< SVzNL3DPosition> a_line;
int ptIdx = 0;
while (getline(inputFile, linedata))
{
if (0 == strncmp("Line_", linedata.c_str(), 5))
{
int ptSize = (int)a_line.size();
if (ptSize > 0)
{
scanData.push_back(a_line);
}
a_line.clear();
ptIdx = 0;
}
else if (0 == strncmp("{", linedata.c_str(), 1))
{
float X, Y, Z;
int imageY = 0;
float leftX, leftY;
float rightX, rightY;
sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
SVzNL3DPosition a_pt;
a_pt.pt3D.x = X;
a_pt.pt3D.y = Y;
a_pt.pt3D.z = Z;
a_pt.nPointIdx = ptIdx;
ptIdx++;
a_line.push_back(a_pt);
}
}
//last line
int ptSize = (int)a_line.size();
if (ptSize > 0)
{
scanData.push_back(a_line);
a_line.clear();
}
inputFile.close();
return;
}
void wd_gridScan_GetROIData(std::vector<std::vector< SVzNL3DPosition>>& scanData, SVzNLRangeD roi_y, std::vector<std::vector< SVzNL3DPosition>>& roiData)
{
int lineNum = (int)scanData.size();
int linePtNum = (int)scanData[0].size();
int globalPtStart = INT_MAX;
int globalPtEnd = 0;
int lineStart = INT_MAX;
int lineEnd = 0;
for (int line = 0; line < lineNum; line++)
{
std::vector< SVzNL3DPosition >& lineData = scanData[line];
int ptSize = (int)lineData.size();
int vldNum = 0;
int ptStart = INT_MAX;
int ptEnd = 0;
for (int i = 0; i < ptSize; i++)
{
if (lineData[i].pt3D.z > 1e-4)
{
if ((lineData[i].pt3D.y < roi_y.min) || (lineData[i].pt3D.y > roi_y.max))
lineData[i].pt3D = { 0.0, 0.0, 0.0 };
}
if (lineData[i].pt3D.z > 1e-4)
{
if (ptStart > i)
ptStart = i;
ptEnd = i;
vldNum++;
}
}
if (vldNum > 0)
{
if (globalPtStart > ptStart)
globalPtStart = ptStart;
if (globalPtEnd < ptEnd)
globalPtEnd = ptEnd;
if (lineStart > line)
lineStart = line;
lineEnd = line;
}
}
int vldLineNum = lineEnd - lineStart + 1;
int vldPtNum = globalPtEnd - globalPtStart + 1;
roiData.resize(vldLineNum);
for (int line = 0; line < vldLineNum; line++)
{
roiData[line].resize(vldPtNum);
for (int i = 0; i < vldPtNum; i++)
roiData[line][i] = scanData[line + lineStart][i + globalPtStart];
}
return;
}
void _outputScanDataFile(char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData,
float lineV, int maxTimeStamp, int clockPerSecond)
{
std::ofstream sw(fileName);
int lineNum = (int)scanData.size();
sw << "LineNum:" << lineNum << std::endl;
sw << "DataType: 0" << std::endl;
sw << "ScanSpeed:" << lineV << std::endl;
sw << "PointAdjust: 1" << std::endl;
sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << std::endl;
for (int line = 0; line < lineNum; line++)
{
int nPositionCnt = (int)scanData[line].size();
sw << "Line_" << line << "_0_" << nPositionCnt << std::endl;
for (int i = 0; i < nPositionCnt; i++)
{
SVzNL3DPosition& pt3D = scanData[line][i];
float x = (float)pt3D.pt3D.x;
float y = (float)pt3D.pt3D.y;
float z = (float)pt3D.pt3D.z;
sw << "{ " << x << "," << y << "," << z << " }-";
sw << "{0,0}-{0,0}" << std::endl;
}
}
sw.close();
}
void _outputPlaneInfo(char* fileName, SSX_planeInfo planeInfo)
{
std::ofstream sw(fileName);
char dataStr[250];
sprintf_s(dataStr, 250, "距离: %g, 方位: %g", planeInfo.distance, planeInfo.dirAngle_deg);
sw << dataStr << std::endl;
sw.close();
}
void _outputRGBDScan_RGBD(
char* fileName,
std::vector<std::vector<SVzNL3DPosition>>& scanLines
)
{
int lineNum = (int)scanLines.size();
std::ofstream sw(fileName);
int realLines = lineNum;
sw << "LineNum:" << realLines << std::endl;
sw << "DataType: 0" << std::endl;
sw << "ScanSpeed: 0" << std::endl;
sw << "PointAdjust: 1" << std::endl;
sw << "MaxTimeStamp: 0_0" << std::endl;
int maxLineIndex = 0;
int max_stamp = 0;
SG_color rgb = { 0, 0, 0 };
SG_color objColor[8] = {
{245,222,179},//淡黄色
{210,105, 30},//巧克力色
{240,230,140},//黄褐色
{135,206,235},//天蓝色
{250,235,215},//古董白
{189,252,201},//薄荷色
{221,160,221},//梅红色
{188,143,143},//玫瑰红色
};
int size = 1;
int lineIdx = 0;
for (int line = 0; line < lineNum; line++)
{
int linePtNum = (int)scanLines[line].size();
if (linePtNum == 0)
continue;
sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
lineIdx++;
for (int i = 0; i < linePtNum; i++)
{
SVzNL3DPosition* pt3D = &scanLines[line][i];
int ptIdx = pt3D->nPointIdx & 0xFFFF;
if (ptIdx > 0)
{
rgb = objColor[ptIdx % 8];
size = 3;
}
else //if (pt3D->nPointIdx == 0)
{
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;
}
}
sw.close();
}
void _outputRGBDScan_superCluster(
char* fileName,
std::vector<std::vector<SVzNL3DPosition>>& scanLines
)
{
int lineNum = (int)scanLines.size();
std::ofstream sw(fileName);
int realLines = lineNum;
sw << "LineNum:" << realLines << std::endl;
sw << "DataType: 0" << std::endl;
sw << "ScanSpeed: 0" << std::endl;
sw << "PointAdjust: 1" << std::endl;
sw << "MaxTimeStamp: 0_0" << std::endl;
int maxLineIndex = 0;
int max_stamp = 0;
SG_color rgb = { 0, 0, 0 };
SG_color objColor[8] = {
{245,222,179},//淡黄色
{210,105, 30},//巧克力色
{240,230,140},//黄褐色
{135,206,235},//天蓝色
{250,235,215},//古董白
{189,252,201},//薄荷色
{221,160,221},//梅红色
{188,143,143},//玫瑰红色
};
int size = 1;
int lineIdx = 0;
for (int line = 0; line < lineNum; line++)
{
int linePtNum = (int)scanLines[line].size();
if (linePtNum == 0)
continue;
sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
lineIdx++;
for (int i = 0; i < linePtNum; i++)
{
SVzNL3DPosition* pt3D = &scanLines[line][i];
int superIdx = pt3D->nPointIdx & 0xffff;
int flag = pt3D->nPointIdx >> 16;
if (flag == 1) //机鼻
{
rgb = { 255, 0, 0 };
size = 10;
}
else if(flag == 2) //机身
{
rgb = { 255, 0, 0 };
size = 5;
}
else if (flag == 4) //发动机
{
rgb = { 255, 255, 0 };
size = 5;
}
else if (superIdx > 0)
{
rgb = objColor[superIdx % 8];
size = 3;
}
else //if (pt3D->nPointIdx == 0)
{
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;
}
}
sw.close();
}
void _outputScanDataFile_vector(char* fileName, std::vector<std::vector<SVzNL3DPosition>>& scanLines, bool removeZeros, int* headNullLines)
{
std::ofstream sw(fileName);
int lineNum = (int)scanLines.size();
if (lineNum == 0)
return;
sw << "LineNum:" << lineNum << std::endl;
sw << "DataType: 0" << std::endl;
sw << "ScanSpeed: 0" << std::endl;
sw << "PointAdjust: 1" << std::endl;
sw << "MaxTimeStamp: 0_0" << std::endl;
int lineIdx = 0;
int null_lines = 0;
bool counterNull = true;
for (int line = 0; line < lineNum; line++)
{
int linePtNum = (int)scanLines[line].size();
if (linePtNum == 0)
continue;
if (true == removeZeros)
{
int vldPtNum = 0;
for (int i = 0; i < linePtNum; i++)
{
if (scanLines[line][i].pt3D.z > 1e-4)
vldPtNum++;
}
linePtNum = vldPtNum;
}
sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
lineIdx++;
bool isNull = true;
for (int i = 0; i < linePtNum; i++)
{
SVzNL3DPoint* pt3D = &scanLines[line][i].pt3D;
if ((pt3D->z > 1e-4) && (isNull == true))
isNull = false;
if ((true == removeZeros) && (pt3D->z < 1e-4))
continue;
float x = (float)pt3D->x;
float y = (float)pt3D->y;
float z = (float)pt3D->z;
sw << "{ " << x << "," << y << "," << z << " }-";
sw << "{0,0}-{0,0}" << std::endl;
}
if (true == counterNull)
{
if (true == isNull)
null_lines++;
else
counterNull = false;
}
}
*headNullLines = null_lines;
sw.close();
}
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;
}
SSX_planeParkingParam _readParkingPara(char* fileName)
{
SSX_planeParkingParam parkingPara;
memset(&parkingPara, 0, sizeof(SSX_planeParkingParam));
std::ifstream inputFile(fileName);
std::string linedata;
if (inputFile.is_open() == false)
return parkingPara;
//调平矩阵
float x, y, z;
std::getline(inputFile, linedata);
sscanf_s(linedata.c_str(), "parkingPoint: { %f, %f, %f }", &x, &y, &z);
parkingPara.parkingPoint = { x, y, z };
std::getline(inputFile, linedata);
sscanf_s(linedata.c_str(), "guideLinePoint: { %f, %f, %f }", &x, &y, &z);
parkingPara.guideLinePoint = { x, y, z };
std::getline(inputFile, linedata);
sscanf_s(linedata.c_str(), "guidingRange: %f", &x);
parkingPara.guidingRange = x;
std::getline(inputFile, linedata);
sscanf_s(linedata.c_str(), "parkingRange: %f", &x);
parkingPara.parkingRange = x;
std::getline(inputFile, linedata);
sscanf_s(linedata.c_str(), "distFromNoseToWheel: %f", &x);
parkingPara.distFromNoseToWheel = x;
inputFile.close();
return parkingPara;
}
#define TEST_COMPUTE_GROUND_PARA 0
#define TEST_COMPUTE_POSITION 1
#define TEST_GROUP 1
int main()
{
const char* dataPath[TEST_GROUP] = {
"F:/ShangGu/项目/水木宏创/停机位停靠引导/数据/20260613_144419-波音737/", //0
};
SVzNLRange fileIdx[TEST_GROUP] = {
{1,43},
};
const char* ver = wd_PlaneLocalizationVersion();
printf("ver:%s\n", ver);
#if TEST_COMPUTE_GROUND_PARA
int cvtGrp = 0;
char _calib_datafile[256];
sprintf_s(_calib_datafile, "%sLaserData_1.txt", dataPath[cvtGrp]);
std::vector<std::vector< SVzNL3DPosition>> scanData;
wdReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanData);
int lineNum = (int)scanData.size();
if (scanData.size() > 0)
{
SSG_planeCalibPara calibPara = wd_getGroundCalibPara(scanData);
char calibFile[250];
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[cvtGrp]);
_outputCalibPara(calibFile, calibPara);
#if 1
for (int fidx = fileIdx[cvtGrp].nMin; fidx <= fileIdx[cvtGrp].nMax; fidx++)
{
//fidx =4;
char _scan_file[256];
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[cvtGrp], fidx);
std::vector<std::vector< SVzNL3DPosition>> scanLines;
wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
if (scanLines.size() == 0)
continue;
double groundHOffset = 200; //去除地面参数
lineNum = (int)scanLines.size();
for (int line = 0; line < lineNum; line++)
{
int linePtNum = scanLines[line].size();
for (int j = 0; j < linePtNum; j++)
{
SVzNL3DPoint a_pt;
a_pt = _ptRotate(scanLines[line][j].pt3D, calibPara.planeCalib);
//if (a_pt.y >= (calibPara.planeHeight - groundHOffset))
// a_pt = { 0.0, 0.0, 0.0 };
scanLines[line][j].pt3D = a_pt;
}
}
sprintf_s(_scan_file, "%sLaserData_%d_ground_calib.txt", dataPath[cvtGrp], fidx);
int headNullLines = 0;
_outputScanDataFile_vector(_scan_file, scanLines, false, &headNullLines);
}
#endif
printf("%s: calib done!\n", _calib_datafile);
}
#endif
#if TEST_COMPUTE_POSITION
for (int grp = 0; grp < TEST_GROUP; grp++)
{
SSG_planeCalibPara groundCalibPara;
//初始化成单位阵
groundCalibPara.planeCalib[0] = 1.0;
groundCalibPara.planeCalib[1] = 0.0;
groundCalibPara.planeCalib[2] = 0.0;
groundCalibPara.planeCalib[3] = 0.0;
groundCalibPara.planeCalib[4] = 1.0;
groundCalibPara.planeCalib[5] = 0.0;
groundCalibPara.planeCalib[6] = 0.0;
groundCalibPara.planeCalib[7] = 0.0;
groundCalibPara.planeCalib[8] = 1.0;
groundCalibPara.planeHeight = -1.0;
for (int i = 0; i < 9; i++)
groundCalibPara.invRMatrix[i] = groundCalibPara.planeCalib[i];
char calibFile[250];
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]);
groundCalibPara = _readCalibPara(calibFile);
sprintf_s(calibFile, "%sparking_para.txt", dataPath[grp]);
SSX_planeParkingParam parkingParam = _readParkingPara(calibFile);
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
{
//fidx =18;
char _scan_file[256];
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
std::vector<std::vector< SVzNL3DPosition>> scanLines;
wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
//转成plyTxt格式
//sprintf_s(_scan_file, "%s%d_ply_Hi229229.txt", dataPath[grp], fidx);
//wdSavePlyTxt(_scan_file, scanLines);
long t1 = (long)GetTickCount64();//统计时间
SSG_treeGrowParam growParam;
growParam.maxLineSkipNum = 10;
growParam.yDeviation_max = 300.0;
growParam.maxSkipDistance = 300.0;
growParam.zDeviation_max = 300.0;//
growParam.minLTypeTreeLen = 500; //mm
growParam.minVTypeTreeLen = 500; //mm
int errCode = 0;
#if _OUTPUT_DEBUG_DATA
std::vector< std::vector<SVzNL3DPosition>> debugData;
#endif
SSX_planeInfo planeInfo = wd_planeLocalization(
scanLines,
groundCalibPara,
parkingParam,
growParam,
#if _OUTPUT_DEBUG_DATA
debugData,
#endif
& errCode);
long t2 = (long)GetTickCount64();
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
//输出测试结果
#if _OUTPUT_DEBUG_DATA
sprintf_s(_scan_file, "%sresult\\%d_superCluster.txt", dataPath[grp], fidx);
_outputRGBDScan_superCluster(_scan_file, debugData);
#endif
sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx);
_outputRGBDScan_RGBD(_scan_file, scanLines);
sprintf_s(_scan_file, "%sresult\\%d_fillingPort_info.txt", dataPath[grp], fidx);
_outputPlaneInfo(_scan_file, planeInfo);
}
}
#endif
}
// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
// 调试程序: F5 或调试 >“开始调试”菜单
// 入门使用技巧:
// 1. 使用解决方案资源管理器窗口添加/管理文件
// 2. 使用团队资源管理器窗口连接到源代码管理
// 3. 使用输出窗口查看生成输出和其他消息
// 4. 使用错误列表窗口查看错误
// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件

145
HC_planeLocalization_test/HC_planeLocalization_test.vcxproj Normal file
View File

@ -0,0 +1,145 @@
<?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>17.0</VCProjectVersion>
<Keyword>Win32Proj</Keyword>
<ProjectGuid>{e59226fe-e1fb-4f3b-9a93-a4173be316b8}</ProjectGuid>
<RootNamespace>HCplaneLocalizationtest</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>v143</PlatformToolset>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
<ConfigurationType>Application</ConfigurationType>
<UseDebugLibraries>false</UseDebugLibraries>
<PlatformToolset>v143</PlatformToolset>
<WholeProgramOptimization>true</WholeProgramOptimization>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
<ConfigurationType>Application</ConfigurationType>
<UseDebugLibraries>true</UseDebugLibraries>
<PlatformToolset>v143</PlatformToolset>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
<ConfigurationType>Application</ConfigurationType>
<UseDebugLibraries>false</UseDebugLibraries>
<PlatformToolset>v143</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|x64'">
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
</PropertyGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
<ClCompile>
<WarningLevel>Level3</WarningLevel>
<SDLCheck>true</SDLCheck>
<PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<ConformanceMode>true</ConformanceMode>
</ClCompile>
<Link>
<SubSystem>Console</SubSystem>
<GenerateDebugInformation>true</GenerateDebugInformation>
</Link>
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
<ClCompile>
<WarningLevel>Level3</WarningLevel>
<FunctionLevelLinking>true</FunctionLevelLinking>
<IntrinsicFunctions>true</IntrinsicFunctions>
<SDLCheck>true</SDLCheck>
<PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<ConformanceMode>true</ConformanceMode>
</ClCompile>
<Link>
<SubSystem>Console</SubSystem>
<GenerateDebugInformation>true</GenerateDebugInformation>
</Link>
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
<ClCompile>
<WarningLevel>Level3</WarningLevel>
<SDLCheck>true</SDLCheck>
<PreprocessorDefinitions>_CRT_SECURE_NO_WARNINGS;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<ConformanceMode>true</ConformanceMode>
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;</AdditionalIncludeDirectories>
</ClCompile>
<Link>
<SubSystem>Console</SubSystem>
<GenerateDebugInformation>true</GenerateDebugInformation>
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
<AdditionalDependencies>opencv_world320d.lib;baseAlgorithm.lib;HC_planeLocalization.lib;%(AdditionalDependencies)</AdditionalDependencies>
</Link>
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
<ClCompile>
<WarningLevel>Level3</WarningLevel>
<FunctionLevelLinking>true</FunctionLevelLinking>
<IntrinsicFunctions>true</IntrinsicFunctions>
<SDLCheck>true</SDLCheck>
<PreprocessorDefinitions>_CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<ConformanceMode>true</ConformanceMode>
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;</AdditionalIncludeDirectories>
</ClCompile>
<Link>
<SubSystem>Console</SubSystem>
<GenerateDebugInformation>true</GenerateDebugInformation>
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
<AdditionalDependencies>opencv_world320.lib;baseAlgorithm.lib;HC_planeLocalization.lib;%(AdditionalDependencies)</AdditionalDependencies>
</Link>
</ItemDefinitionGroup>
<ItemGroup>
<ClCompile Include="HC_planeLocalization_test.cpp" />
</ItemGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
<ImportGroup Label="ExtensionTargets">
</ImportGroup>
</Project>

5
sourceCode/SG_errCode.h
View File

@ -52,3 +52,8 @@
//蓼淘보룹땍貫
#define SX_ERR_NO_HIGHEST_ROD -2701
//飞机停靠引导
#define SX_ERR_NO_PLANE_FOUND -2801
#define SX_ERR_NOSEPOINT_FAIL -2802
#define SX_ERR_ENDINE_FAIL -2803

740
sourceCode/planeLocalization.cpp Normal file
View File

@ -0,0 +1,740 @@
#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "planeLocalization_Export.h"
#include <opencv2/opencv.hpp>
#include <limits>
//version 1.0.0 : base version release to customer
std::string m_strVersion = " PlaneLocalization 1.0.0";
const char* wd_PlaneLocalizationVersion(void)
{
return m_strVersion.c_str();
}
//使用RANSAC方法云计算地面参数。
SSG_planeCalibPara wd_getGroundCalibPara(
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
//设置初始结果
double initCalib[9] = {
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0 };
SSG_planeCalibPara groundParam;
for (int i = 0; i < 9; i++)
{
groundParam.planeCalib[i] = initCalib[i];
groundParam.invRMatrix[i] = initCalib[i];
}
groundParam.planeHeight = -1.0;
std::vector<cv::Point3d> points;
for (int line = 0; line < (int)scanLines.size(); line++)
{
for (int j = 0, j_max = (int)scanLines[line].size(); j < j_max; j++)
{
if ( (scanLines[line][j].pt3D.z > 1e-4) && (scanLines[line][j].pt3D.z <200000)) //200米内的数据
{
cv::Point3d a_pt = cv::Point3d(scanLines[line][j].pt3D.x, scanLines[line][j].pt3D.y, scanLines[line][j].pt3D.z);
points.push_back(a_pt);
}
}
}
double distTh = 1.0;
std::vector<cv::Point3d> in_inliers;
Plane groundPlane = ransacFitPlane(points, in_inliers, distTh);
if (in_inliers.size() < 100)
{
return groundParam;
}
SVzNL3DPoint vec_1;
if (groundPlane.B > 0)
vec_1 = { groundPlane.A, groundPlane.B, groundPlane.C };
else
vec_1 = { -groundPlane.A, -groundPlane.B, -groundPlane.C };
SVzNL3DPoint vec_z = { 0, 1.0, 0.0 };
groundParam = wd_computeRTMatrix(vec_1, vec_z);
//计算地面高度
std::vector<cv::Point3d> groundPoints;
for (int i = 0; i < (int)in_inliers.size(); i++)
{
cv::Point3d rPt;
rPt.x = in_inliers[i].x * groundParam.planeCalib[0] + in_inliers[i].y * groundParam.planeCalib[1] + in_inliers[i].z * groundParam.planeCalib[2];
rPt.y = in_inliers[i].x * groundParam.planeCalib[3] + in_inliers[i].y * groundParam.planeCalib[4] + in_inliers[i].z * groundParam.planeCalib[5];
rPt.z = in_inliers[i].x * groundParam.planeCalib[6] + in_inliers[i].y * groundParam.planeCalib[7] + in_inliers[i].z * groundParam.planeCalib[8];
groundPoints.push_back(rPt);
}
double groundY = 0;
for (int i = 0; i < (int)groundPoints.size(); i++)
groundY += groundPoints[i].y;
groundY = groundY / (int)groundPoints.size();
groundParam.planeHeight = groundY;
return groundParam;
}
int _checkAdjacency(
std::vector< SVzNL3DPosition>& cluster_1,
const int cluster1_idx, const int cluster2_idx,
std::vector<std::vector<int>>& clusterMask)
{
int lineNum = (int)clusterMask.size();
int linePtNum = (int)clusterMask[0].size();
int cluster1_size = (int)cluster_1.size();
int adjacentNum = 0;
for (int i = 0; i < cluster1_size; i++)
{
SVzNL3DPosition& a_pt = cluster_1[i];
//检查相邻
int lineIdx = a_pt.nPointIdx >> 16;
int ptIdx = a_pt.nPointIdx & 0xFFFF;
if (clusterMask[lineIdx][ptIdx] != cluster1_idx)
continue;
int leftCol = lineIdx - 1;
int rightCol = lineIdx + 1;
int topRow = ptIdx - 1;
int btmRow = ptIdx + 1;
if ((leftCol < 0) || (rightCol >= lineNum) || (topRow < 0) || (btmRow >= linePtNum))
continue;
if (clusterMask[leftCol][ptIdx] == cluster2_idx)
adjacentNum++;
if (clusterMask[rightCol][ptIdx] == cluster2_idx)
adjacentNum++;
if (clusterMask[lineIdx][topRow] == cluster2_idx)
adjacentNum++;
if (clusterMask[lineIdx][btmRow] == cluster2_idx)
adjacentNum++;
}
return adjacentNum;
}
void _searchSuperCluster(
std::vector<int>& a_superCluster,
std::vector<std::vector<int>>& adjacencyScoreTable,
std::vector<int>& clusterFlags,
std::vector<SVzNL3DRangeD>& clusterROIs,
const int adjacencyScoreTh,
const double adjacencyZDistTh)
{
int clusterNum = adjacencyScoreTable.size();
int searchIdx = 0;
int seedIdx = a_superCluster[0];
clusterFlags[seedIdx] = 1;
while (searchIdx < a_superCluster.size())
{
int clusterIdx = a_superCluster[searchIdx];
SVzNL3DRangeD& seedROI = clusterROIs[clusterIdx];
for (int i = 0; i < clusterNum; i++)
{
if ((clusterFlags[i] > 0) || (i == seedIdx))
continue;
int score = adjacencyScoreTable[clusterIdx][i];
if (score > adjacencyScoreTh)
{
a_superCluster.push_back(i);
clusterFlags[i] = 1;
}
else if (score > 0)
{
//计算距离:使用Z距离
SVzNL3DRangeD& chkROI = clusterROIs[i];
double zDist;
if (seedROI.zRange.min > chkROI.zRange.max)
zDist = seedROI.zRange.min - chkROI.zRange.max;
else if (chkROI.zRange.min > seedROI.zRange.max)
zDist = chkROI.zRange.min - seedROI.zRange.max;
else
zDist = 0; //重叠
if (zDist < adjacencyZDistTh)
{
a_superCluster.push_back(i);
clusterFlags[i] = 1;
}
}
}
searchIdx++;
}
}
void _updateRoi3D(SVzNL3DRangeD& roi, SVzNL3DPoint& a_pt)
{
if (a_pt.z > 1E-4)
{
if (roi.zRange.max < 0)
{
roi.xRange.min = a_pt.x;
roi.xRange.max = a_pt.x;
roi.yRange.min = a_pt.y;
roi.yRange.max = a_pt.y;
roi.zRange.min = a_pt.z;
roi.zRange.max = a_pt.z;
}
else
{
if (roi.xRange.min > a_pt.x)
roi.xRange.min = a_pt.x;
if (roi.xRange.max < a_pt.x)
roi.xRange.max = a_pt.x;
if (roi.yRange.min > a_pt.y)
roi.yRange.min = a_pt.y;
if (roi.yRange.max < a_pt.y)
roi.yRange.max = a_pt.y;
if (roi.zRange.min > a_pt.z)
roi.zRange.min = a_pt.z;
if (roi.zRange.max < a_pt.z)
roi.zRange.max = a_pt.z;
}
}
return;
}
SVzNL3DRangeD _getSupcluster(std::vector<int>& superClusterIndice, std::vector<SVzNL3DRangeD>& objClustersROIs)
{
int idx_0 = superClusterIndice[0];
SVzNL3DRangeD mergeROI = objClustersROIs[idx_0];
for (int i = 1; i < (int)superClusterIndice.size(); i++)
{
int idx = superClusterIndice[i];
SVzNL3DRangeD& a_roi = objClustersROIs[idx];
mergeROI.xRange.min = mergeROI.xRange.min > a_roi.xRange.min ? a_roi.xRange.min : mergeROI.xRange.min;
mergeROI.xRange.max = mergeROI.xRange.max < a_roi.xRange.max ? a_roi.xRange.max : mergeROI.xRange.max;
mergeROI.yRange.min = mergeROI.yRange.min > a_roi.yRange.min ? a_roi.yRange.min : mergeROI.yRange.min;
mergeROI.yRange.max = mergeROI.yRange.max < a_roi.yRange.max ? a_roi.yRange.max : mergeROI.yRange.max;
mergeROI.zRange.min = mergeROI.zRange.min > a_roi.zRange.min ? a_roi.zRange.min : mergeROI.zRange.min;
mergeROI.zRange.max = mergeROI.zRange.max < a_roi.zRange.max ? a_roi.zRange.max : mergeROI.zRange.max;
}
return mergeROI;
}
SSX_planeInfo wd_planeLocalization(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSG_planeCalibPara groundCalibParam,
const SSX_planeParkingParam parkingParam,
const SSG_treeGrowParam growParam,
#if _OUTPUT_DEBUG_DATA
std::vector< std::vector<SVzNL3DPosition>>& debugData,
#endif
int* errCode)
{
*errCode = 0;
//内部参数
double planeMinHeight = 4000; //最小高度4米
double planeMinWidth = 25000; //最小宽度2.5米
double nearFarTh = 80000.0; //远近分界
double groundHOffset = 200; //去除地面参数
double bodyHeadJudge_lenTh = 20000.0; //机头与机身判断的长度门限。20米
SVzNLRangeD bodyRangeToNose = { -2000.0, 1000.0 }; //以机鼻高度为基准，定义机身的高度范围
SVzNLRangeD sameRBodyRange = { 5000.0, 28000.0 }; //机身圆桶段，以距离机鼻距离为基准
SVzNLRangeD engineToNoseDistRange = { 12000.0, 16000.0 };
SVzNLRangeD bodyYRange = { groundCalibParam.planeHeight - 4500.0, groundCalibParam.planeHeight - 1500 }; //机身的Y范围
SSX_planeInfo planePoseInfo;
memset(&planePoseInfo, 0, sizeof(SSX_planeInfo));
int lineNum = (int)scanLines.size();
if (lineNum == 0)
{
*errCode = SG_ERR_3D_DATA_NULL;
return planePoseInfo;
}
int linePtNum = (int)scanLines[0].size();
//判断数据格式是否为grid。算法只能处理grid数据格式
bool isGridData = true;
for (int line = 0; line < lineNum; line++)
{
if (linePtNum != (int)scanLines[line].size())
{
isGridData = false;
break;
}
}
if (false == isGridData)//数据不是网格格式
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return planePoseInfo;
}
//将引导线指向Y轴
SVzNL3DPoint guideLineVec = { parkingParam.guideLinePoint.x - parkingParam.parkingPoint.x , 0, parkingParam.guideLinePoint.z - parkingParam.parkingPoint.z};
if (guideLineVec.z < 0)
guideLineVec = {-guideLineVec.x, -guideLineVec.y, -guideLineVec.z};
guideLineVec = vec3_normalize(guideLineVec);
SVzNL3DPoint targetVec = { 0, 0, 1 };
SSG_planeCalibPara guideLineRotatePara = wd_computeRTMatrix(guideLineVec, targetVec);
//对正引导线
SVzNL3DPoint rotateParkingPoint = wd_ptRotate(parkingParam.parkingPoint, guideLineRotatePara.planeCalib);
SVzNL3DPoint chkPoint = wd_ptRotate(parkingParam.guideLinePoint, guideLineRotatePara.planeCalib);
//计算目标范围
SVzNLRangeD ROI_x_near = { rotateParkingPoint.x - parkingParam.parkingRange, rotateParkingPoint.x + parkingParam.parkingRange };
SVzNLRangeD ROI_x_far = { rotateParkingPoint.x - parkingParam.parkingRange * 3, rotateParkingPoint.x + parkingParam.parkingRange* 3 }; //大于100m外，需要放宽，因为飞机正在进引导线
SVzNLRangeD ROI_z = { rotateParkingPoint.z - parkingParam.distFromNoseToWheel - 2000.0, rotateParkingPoint.z + parkingParam.guidingRange };
//旋转， ROI过滤
std::vector< std::vector<SVzNL3DPosition>> rotateROIData;
rotateROIData.resize(lineNum);
for (int line = 0; line < lineNum; line++)
{
rotateROIData[line].resize(linePtNum);
for (int j = 0; j < linePtNum; j++)
{
SVzNL3DPosition a_pt;
a_pt.nPointIdx = 0;
a_pt.pt3D = wd_ptRotate(scanLines[line][j].pt3D, groundCalibParam.planeCalib);
if (a_pt.pt3D.y >= (groundCalibParam.planeHeight - groundHOffset))
a_pt.pt3D = { 0.0, 0.0, 0.0 };
//再次旋转，将引导线指向Y轴
a_pt.pt3D = wd_ptRotate(a_pt.pt3D, guideLineRotatePara.planeCalib);
rotateROIData[line][j] = a_pt;
}
}
std::vector< std::vector<SWD_poloarScan2D>> polarScanData; //扫描数据：扫描水平角、垂直角和距离,
polarScanData.resize(lineNum);
std::vector<SVzNL3DPosition> validPoints;
for (int line = 0; line < lineNum; line++)
{
polarScanData[line].resize(rotateROIData[line].size());
for (int j = 0, j_max = (int)rotateROIData[line].size(); j < j_max; j++)
{
if (rotateROIData[line][j].pt3D.z > 1e-4) //去除地面后的数据
{
SVzNL3DPosition a_pt;
a_pt.nPointIdx = (line << 16) | (j & 0xffff);
a_pt.pt3D = rotateROIData[line][j].pt3D;
validPoints.push_back(a_pt);
//计算扫描角度：使用旋转前数据
a_pt = scanLines[line][j];
SWD_poloarScan2D a_polarData;
a_polarData.range_mm = sqrt(pow(a_pt.pt3D.x, 2) + pow(a_pt.pt3D.y, 2) + pow(a_pt.pt3D.z, 2));
//水平角：
double yaw_rad = atan2(-a_pt.pt3D.x, a_pt.pt3D.z);
a_polarData.yaw_deg = yaw_rad * 180 / PI;
//垂直角：
double pitch_rad = asin(-a_pt.pt3D.y / a_polarData.range_mm);
a_polarData.pitch_deg = pitch_rad * 180 / PI;
polarScanData[line][j] = a_polarData;
}
else
{
polarScanData[line][j].range_mm = 0;
polarScanData[line][j].yaw_deg = 0;
polarScanData[line][j].pitch_deg = 0;
}
scanLines[line][j].nPointIdx = 0;
}
}
#if _OUTPUT_DEBUG_DATA
//输出投影数据
debugData.clear();
debugData.resize(scanLines.size());
for (int line = 0; line < lineNum; line++)
{
debugData[line].resize(scanLines[line].size());
for (int j = 0, j_max = (int)scanLines[line].size(); j < j_max; j++)
{
debugData[line][j].nPointIdx = 0;
debugData[line][j].pt3D = rotateROIData[line][j].pt3D;
}
}
#endif
//聚类
//使用SVzNL3DPosition的nPointIdx表示2D信息（高16位Line， 低16位ptIdx）
//搜索时搜索邻域以加速
int clusterCheckWin = 5;
double clusterDist = sqrt(pow(growParam.zDeviation_max, 2) + pow(growParam.maxSkipDistance, 2) + pow(growParam.yDeviation_max, 2));
int distType = 1; //0 - 2d distance; 1- 3d distance
std::vector<std::vector< SVzNL3DPosition>> allClusters;
wd_pointClustering_speedUp(
validPoints,
lineNum, linePtNum, clusterCheckWin, //搜索窗口
clusterDist,
distType, //0 - 2d distance; 1- 3d distance
allClusters //result
);
//统计cluster的ROI
std::vector<SVzNL3DRangeD> allClusterROIs;
allClusterROIs.resize(allClusters.size());
for (int m = 0; m < (int)allClusters.size(); m++)
{
SVzNL3DRangeD a_roi3D;
memset(&a_roi3D, 0, sizeof(SVzNL3DRangeD));
a_roi3D.zRange.max = -1;
for (int i = 0; i < (int)allClusters[m].size(); i++)
_updateRoi3D(a_roi3D, allClusters[m][i].pt3D);
allClusterROIs[m] = a_roi3D;
}
//ROI过滤
std::vector<std::vector< SVzNL3DPosition>> objClusters;
std::vector<SVzNL3DRangeD> objClustersROIs;
for (int m = 0; m < (int)allClusters.size(); m++)
{
SVzNL3DRangeD& a_roi = allClusterROIs[m];
if ((a_roi.zRange.min - rotateParkingPoint.z) < nearFarTh)
{
if ((a_roi.xRange.min >= ROI_x_near.min) && (a_roi.xRange.max <= ROI_x_near.max) &&
(a_roi.zRange.min >= ROI_z.min) && (a_roi.zRange.max <= ROI_z.max))
{
objClusters.push_back(allClusters[m]);
objClustersROIs.push_back(a_roi);
}
}
else
{
if ((a_roi.xRange.min >= ROI_x_far.min) && (a_roi.xRange.max <= ROI_x_far.max) &&
(a_roi.zRange.min >= ROI_z.min) && (a_roi.zRange.max <= ROI_z.max))
{
objClusters.push_back(allClusters[m]);
objClustersROIs.push_back(a_roi);
}
}
}
//分析聚类之间的关系
//建立聚类Mask
int clusterNum = (int)objClusters.size();
std::vector<std::vector<int>> clusterMask;
clusterMask.resize(lineNum);
for (int i = 0; i < lineNum; i++)
{
clusterMask[i].resize(linePtNum);
std::fill(clusterMask[i].begin(), clusterMask[i].end(), -1);
}
for (int i = 0; i < clusterNum; i++)
{
std::vector< SVzNL3DPosition>& a_cluster = objClusters[i];
int ptSize = (int)a_cluster.size();
for (int j = 0; j < ptSize; j++)
{
SVzNL3DPosition& a_pt = a_cluster[j];
int lineIdx = a_pt.nPointIdx >> 16;
int ptIdx = a_pt.nPointIdx & 0xffff;
clusterMask[lineIdx][ptIdx] = i;
}
}
//构建聚类的相邻关系表
std::vector<std::vector<int>> adjacencyScoreTable;
adjacencyScoreTable.resize(clusterNum);
for (int i = 0; i < clusterNum; i++)
adjacencyScoreTable[i].resize(clusterNum);
for (int i = 0; i < clusterNum; i++)
{
std::vector< SVzNL3DPosition>& cluster_1 = objClusters[i];
for (int j = i + 1; j < clusterNum; j++)
{
int score = _checkAdjacency(cluster_1, i, j, clusterMask);
adjacencyScoreTable[i][j] = score;
adjacencyScoreTable[j][i] = score;
}
}
//二次聚类成超聚类
int adjacencyScoreTh = 10;
double adjacencyZDistTh = 18000.0;//18米长
std::vector<std::vector<int>> superClusers; //超级聚类，使用聚类的ID
std::vector<int> clusterFlags;
clusterFlags.resize(objClusters.size());
std::fill(clusterFlags.begin(), clusterFlags.end(), -1);
for (int i = 0; i < (int)objClusters.size(); i++)
{
if (clusterFlags[i] >= 0)
continue;
std::vector<int> a_superCluster;
a_superCluster.push_back(i);
//递归搜索
_searchSuperCluster(a_superCluster, adjacencyScoreTable, clusterFlags, objClustersROIs, adjacencyScoreTh, adjacencyZDistTh);
//计算ROI
SVzNL3DRangeD superROI = _getSupcluster(a_superCluster, objClustersROIs);
double h = abs(superROI.yRange.min - groundCalibParam.planeHeight);
double w = superROI.xRange.max - superROI.xRange.min;
double len = superROI.zRange.max - superROI.zRange.min;
if( (h > planeMinHeight) && (w > planeMinWidth))
superClusers.push_back(a_superCluster);
}
#if _OUTPUT_DEBUG_DATA
//标注
for (int i = 0; i < (int)superClusers.size(); i++)
{
std::vector<int>& a_superCluster = superClusers[i];
int memberSize = (int)a_superCluster.size();
for (int m = 0; m < memberSize; m++)
{
int clusterIdx = a_superCluster[m];
std::vector< SVzNL3DPosition>& a_cluster = objClusters[clusterIdx];
int ptSize = (int)a_cluster.size();
for (int j = 0; j < ptSize; j++)
{
SVzNL3DPosition& a_pt = a_cluster[j];
int lineIdx = a_pt.nPointIdx >> 16;
int ptIdx = a_pt.nPointIdx & 0xffff;
debugData[lineIdx][ptIdx].nPointIdx += (i + 1);
}
}
}
#endif
if (superClusers.size() == 0)
{
*errCode = SX_ERR_NO_PLANE_FOUND;
return planePoseInfo;
}
//挑选出点数最多的超聚类作为飞机
std::vector<int> superCluserPointSizes;
superCluserPointSizes.resize(superClusers.size());
for (int i = 0; i < (int)superClusers.size(); i++)
{
std::vector<int>& a_superCluster = superClusers[i];
int superSize = 0;
int memberSize = (int)a_superCluster.size();
for (int m = 0; m < memberSize; m++)
{
int clusterIdx = a_superCluster[m];
std::vector< SVzNL3DPosition>& a_cluster = objClusters[clusterIdx];
superSize += (int)a_cluster.size();
}
superCluserPointSizes[i] = superSize;
}
int bestId = 0;
for (int i = 1; i < (int)superCluserPointSizes.size(); i++)
{
if (superCluserPointSizes[bestId] < superCluserPointSizes[i])
bestId = i;
}
std::vector<int>& planeSuperCluster = superClusers[bestId];
int planeClusterSize = (int)planeSuperCluster.size();
//寻找机头位置: ROI最靠前（Z最小）
int noseClusterId = -1;
for (int i = 0; i < planeClusterSize; i++)
{
int clusterId = planeSuperCluster[i];
SVzNL3DRangeD& a_roi = objClustersROIs[clusterId];
if ((a_roi.yRange.min < bodyYRange.min) && (a_roi.yRange.max > bodyYRange.max)) //处于机身的Y范围内
{
if(noseClusterId < 0)
noseClusterId = clusterId;
else if (objClustersROIs[noseClusterId].zRange.min > objClustersROIs[clusterId].zRange.min)
noseClusterId = clusterId;
}
}
if (noseClusterId < 0)
{
*errCode = SX_ERR_NOSEPOINT_FAIL;
return planePoseInfo;
}
//搜索机鼻点
SVzNL3DPosition nosePoint;
nosePoint.nPointIdx = 0;
nosePoint.pt3D = { 0.0, 0.0, 0.0 };
for (int i = 0; i < (int)objClusters[noseClusterId].size(); i++)
{
if (nosePoint.pt3D.z < 1e-4)
nosePoint = objClusters[noseClusterId][i];
else if(nosePoint.pt3D.z > objClusters[noseClusterId][i].pt3D.z)
nosePoint = objClusters[noseClusterId][i];
}
if (nosePoint.pt3D.z < 1e-4)
{
*errCode = SX_ERR_NOSEPOINT_FAIL;
return planePoseInfo;
}
#if _OUTPUT_DEBUG_DATA
{
int nose_lineIdx = nosePoint.nPointIdx >> 16;
int nose_ptIdx = nosePoint.nPointIdx & 0xffff;
debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x10000; //机鼻点
}
#endif
//计算姿态
//判断机鼻所在的长度：以机鼻高度为基准，取高度范围内的点，计算长度
SVzNLRangeD bodyHRange = { nosePoint.pt3D.y + bodyRangeToNose.min, nosePoint.pt3D.y + bodyRangeToNose.max };
std::vector<SVzNL3DPosition> bodyData;
for (int i = 0; i < (int)objClusters[noseClusterId].size(); i++)
{
if ((objClusters[noseClusterId][i].pt3D.y >= bodyHRange.min) && (objClusters[noseClusterId][i].pt3D.y <= bodyHRange.max))
bodyData.push_back(objClusters[noseClusterId][i]);
}
//计算到机鼻最长距离
double maxDistanceToNose = 0;
std::vector<SVzNL2DPointD> XOZBodayProjectionData;
std::vector<SVzNL3DPosition> XOZBodayData;
for (int i = 0; i < (int)bodyData.size(); i++)
{
double dist = sqrt(pow(nosePoint.pt3D.x - bodyData[i].pt3D.x, 2) + pow(nosePoint.pt3D.z - bodyData[i].pt3D.z, 2));
if (maxDistanceToNose < dist)
maxDistanceToNose = dist;
if ((dist >= sameRBodyRange.min) && (dist <= sameRBodyRange.max))
{
XOZBodayData.push_back(bodyData[i]);
SVzNL2DPointD a_projection = { bodyData[i].pt3D.x, bodyData[i].pt3D.z };
XOZBodayProjectionData.push_back(a_projection);
}
}
if (maxDistanceToNose > bodyHeadJudge_lenTh) //机身
{
//取机身数据，PCA方法计算姿态
// 二维PCA，求平面内条纹方向
SVzNL2DPointD axis, centroid;
pca2D(XOZBodayProjectionData, axis, centroid);
if (axis.y < 0)
axis = { -axis.x, -axis.y };
double dirAngle = atan(axis.x / axis.y) * 180.0 / PI;
planePoseInfo.nosePoint = nosePoint.pt3D;
planePoseInfo.axis = { axis.x, 0, axis.y };
planePoseInfo.axis = vec3_normalize(planePoseInfo.axis);
planePoseInfo.distance = nosePoint.pt3D.z - rotateParkingPoint.z + parkingParam.distFromNoseToWheel;
planePoseInfo.deviation = nosePoint.pt3D.x - rotateParkingPoint.x; //偏离引导线
planePoseInfo.dirAngle_deg = dirAngle; //方向角度
#if _OUTPUT_DEBUG_DATA
for(int m = 0; m <(int)XOZBodayData.size(); m ++)
{
int nose_lineIdx = XOZBodayData[m].nPointIdx >> 16;
int nose_ptIdx = XOZBodayData[m].nPointIdx & 0xffff;
if( (debugData[nose_lineIdx][nose_ptIdx].nPointIdx & 0xffff0000) == 0)
debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x20000; //机身点
}
#endif
}
else //机头
{
//取左右发动机数据
std::vector<std::vector<SVzNL3DPosition>> distanceValidData;
distanceValidData.resize(planeClusterSize);
for (int idx = 0; idx < planeClusterSize; idx++)
{
int clusterIdx = planeSuperCluster[idx];
if (clusterIdx == noseClusterId)
continue;
for (int i = 0; i < (int)objClusters[clusterIdx].size(); i++)
{
double dist = sqrt(pow(nosePoint.pt3D.x - objClusters[clusterIdx][i].pt3D.x, 2) + pow(nosePoint.pt3D.z - objClusters[clusterIdx][i].pt3D.z, 2));
if ((dist >= engineToNoseDistRange.min) && (dist <= engineToNoseDistRange.max))
distanceValidData[idx].push_back(objClusters[clusterIdx][i]);
}
}
//计算ROI
std::vector<SVzNLRangeD> dataROIs;
std::vector<int> validFlags;
dataROIs.resize(planeClusterSize);
validFlags.resize(planeClusterSize);
int validNum = 0;
for (int idx = 0; idx < planeClusterSize; idx++)
{
if (distanceValidData[idx].size() == 0)
{
validFlags[idx] = 0;
dataROIs[idx] = { 0.0, 0.0 };
}
else
{
validFlags[idx] = 1;
validNum++;
SVzNLRangeD xRng = { distanceValidData[idx][0].pt3D.x,distanceValidData[idx][0].pt3D.x };
for (int i = 1; i < (int)distanceValidData[idx].size(); i++)
{
xRng.min = xRng.min > distanceValidData[idx][i].pt3D.x ? distanceValidData[idx][i].pt3D.x : xRng.min;
xRng.max = xRng.max < distanceValidData[idx][i].pt3D.x ? distanceValidData[idx][i].pt3D.x : xRng.max;
}
dataROIs[idx] = xRng;
}
}
if (validNum < 2)
{
*errCode = SX_ERR_ENDINE_FAIL;
return planePoseInfo;
}
//取左右Engine
int leftEngineIdx = -1;
int rightEngineIdx = -1;
for (int idx = 0; idx < planeClusterSize; idx++)
{
if (validFlags[idx] == 0)
continue;
if (leftEngineIdx < 0)
leftEngineIdx = idx;
else if (dataROIs[leftEngineIdx].min > dataROIs[idx].min)
leftEngineIdx = idx;
if (rightEngineIdx < 0)
rightEngineIdx = idx;
else if (dataROIs[rightEngineIdx].max < dataROIs[idx].max)
rightEngineIdx = idx;
}
if (leftEngineIdx == rightEngineIdx)
{
*errCode = SX_ERR_ENDINE_FAIL;
return planePoseInfo;
}
//取左右点集的最低点，作为左右发动机的参考点
SVzNL3DPosition leftEnginePoint = distanceValidData[leftEngineIdx][0];
for (int i = 1; i < (int)distanceValidData[leftEngineIdx].size(); i++)
{
if (leftEnginePoint.pt3D.y < distanceValidData[leftEngineIdx][i].pt3D.y)
leftEnginePoint = distanceValidData[leftEngineIdx][i];
}
SVzNL3DPosition rightEnginePoint = distanceValidData[rightEngineIdx][0];
for (int i = 1; i < (int)distanceValidData[rightEngineIdx].size(); i++)
{
if (rightEnginePoint.pt3D.y < distanceValidData[rightEngineIdx][i].pt3D.y)
rightEnginePoint = distanceValidData[rightEngineIdx][i];
}
#if _OUTPUT_DEBUG_DATA
{
int nose_lineIdx = leftEnginePoint.nPointIdx >> 16;
int nose_ptIdx = leftEnginePoint.nPointIdx & 0xffff;
debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x40000; //机鼻点
nose_lineIdx = rightEnginePoint.nPointIdx >> 16;
nose_ptIdx = rightEnginePoint.nPointIdx & 0xffff;
debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x40000; //机鼻点
}
#endif
//计算姿态
//axis与左右发动机连续垂直, 采用(-y,, x)形式
SVzNL2DPointD axis = { -(rightEnginePoint.pt3D.z - leftEnginePoint.pt3D.z), rightEnginePoint.pt3D.x - leftEnginePoint.pt3D.x };
if (axis.y < 0)
axis = { -axis.x, -axis.y};
double dirAngle = atan(axis.x / axis.y) * 180.0 / PI;
planePoseInfo.nosePoint = nosePoint.pt3D;
planePoseInfo.axis = { axis.x, 0, axis.y };
planePoseInfo.axis = vec3_normalize(planePoseInfo.axis);
planePoseInfo.distance = nosePoint.pt3D.z - rotateParkingPoint.z + parkingParam.distFromNoseToWheel;
planePoseInfo.deviation = nosePoint.pt3D.x - rotateParkingPoint.x; //偏离引导线
planePoseInfo.dirAngle_deg = dirAngle; //方向角度
}
return planePoseInfo;
}

44
sourceCode/planeLocalization_Export.h Normal file
View File

@ -0,0 +1,44 @@
#pragma once
#include "SG_algo_Export.h"
#include <vector>
#define _OUTPUT_DEBUG_DATA 1
typedef struct
{
double distance; //距离停机点距离
double deviation; //偏离引导线距离
double dirAngle_deg; //前进角， 以度为单位
SVzNL3DPoint nosePoint;
SVzNL3DPoint axis;
}SSX_planeInfo;
typedef struct
{
SVzNL3DPoint parkingPoint; //停机点（需经过地面调平处理）
SVzNL3DPoint guideLinePoint; //停机引导线上点（需经过地面调平处理），用于确定引导线方向。方向为从停机点到引导线上点的连线。距离停机点20m以上
double guidingRange; //飞机引导范围， 以停机点为基准。超出此范围的目标不被处理
double parkingRange; //飞机距离引导线距离，用于过滤无效目标， 以引导线为基准
double distFromNoseToWheel; //停机点为轮子地点，此参数用于计算机鼻停机位置
}SSX_planeParkingParam;
//读版本号
SG_APISHARED_EXPORT const char* wd_PlaneLocalizationVersion(void);
//相机水平安装计算地面调平参数。。
//相机Z轴基本平行地面时，需要以地面为参照，将相机调水平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SG_APISHARED_EXPORT SSG_planeCalibPara wd_getGroundCalibPara(
std::vector< std::vector<SVzNL3DPosition>>& scanLines);
//飞机姿态计算
SG_APISHARED_EXPORT SSX_planeInfo wd_planeLocalization(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSG_planeCalibPara groundCalibParam,
const SSX_planeParkingParam parkingParam,
const SSG_treeGrowParam growParam,
#if _OUTPUT_DEBUG_DATA
std::vector< std::vector<SVzNL3DPosition>>& debugData,
#endif
int* errCode);