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

planeLocalization

Browse Source 
version 1.3.1 : 改进了取发动机点特征点算法
This commit is contained in:
jerryzeng 2026-07-29 23:56:59 +08:00
parent 7342a322d9
commit 6e9cdf27c4
2 changed files with 85 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
HC_planeLocalization_test/HC_planeLocalization_test.cpp
View File

@ -543,22 +543,24 @@ SSX_planeParkingParam _readParkingPara(char* fileName)
#define TEST_COMPUTE_GROUND_PARA 0
#define TEST_COMPUTE_POSITION 1
#define TEST_GROUP 1
#define TEST_GROUP 3
int main()
{
const char* dataPath[TEST_GROUP] = {
"F:/ShangGu/项目/水木宏创/停机位停靠引导/数据/20260613_144419-波音737/", //0
"F:/ShangGu/项目/水木宏创/停机位停靠引导/中心线锥桶数据/", //1
"F:/ShangGu/项目/水木宏创/停机位停靠引导/数据/现场数据/0729夜晚雷达采集数据/", //2
};
SVzNLRange fileIdx[TEST_GROUP] = {
{1,43},
{1,43}, {1,10}, {1, 145}
};
const char* ver = wd_PlaneLocalizationVersion();
printf("ver:%s\n", ver);
#if TEST_COMPUTE_GROUND_PARA
int cvtGrp = 0;
int cvtGrp = 2;
char _calib_datafile[256];
sprintf_s(_calib_datafile, "%sLaserData_1.txt", dataPath[cvtGrp]);
std::vector<std::vector< SVzNL3DPosition>> scanData;
@ -608,7 +610,7 @@ int main()
#endif
#if TEST_COMPUTE_POSITION
for (int grp = 0; grp < TEST_GROUP; grp++)
for (int grp = 2; grp < TEST_GROUP; grp++)
{
SSG_planeCalibPara groundCalibPara;
//初始化成单位阵
@ -633,7 +635,7 @@ int main()
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
{
//fidx =31;
//fidx =32;
char _scan_file[256];
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
@ -648,9 +650,9 @@ int main()
SSG_treeGrowParam growParam;
growParam.maxLineSkipNum = 10;
growParam.yDeviation_max = 300.0;
growParam.maxSkipDistance = 300.0;
growParam.zDeviation_max = 300.0;//
growParam.yDeviation_max = 2000.0;// 300.0;
growParam.maxSkipDistance = 2000.0; // 300.0;
growParam.zDeviation_max = 2000.0; // 300.0;//
growParam.minLTypeTreeLen = 500; //mm
growParam.minVTypeTreeLen = 500; //mm

70
sourceCode/planeLocalization.cpp
View File

@ -9,7 +9,8 @@
//version 1.1.0 : 优化了机鼻点提取（迭代），增加了没有飞机的输出
//version 1.2.0 : 修正了回归测试中发现的问题
//version 1.3.0 : 修正了取发动机点特征点的一个Bug
std::string m_strVersion = " PlaneLocalization 1.3.0";
//version 1.3.1 : 改进了取发动机点特征点算法
std::string m_strVersion = " PlaneLocalization 1.3.1";
const char* wd_PlaneLocalizationVersion(void)
{
return m_strVersion.c_str();
@ -432,6 +433,9 @@ SSX_planeInfo wd_planeLocalization(
for (int m = 0; m < (int)allClusters.size(); m++)
{
SVzNL3DRangeD& a_roi = allClusterROIs[m];
double height = groundCalibParam.planeHeight - a_roi.yRange.min;
if (height >= 2000.00) //2米高度门限， 跳过一般人身高
{
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) &&
@ -451,6 +455,7 @@ SSX_planeInfo wd_planeLocalization(
}
}
}
}
//分析聚类之间的关系
//建立聚类Mask
@ -766,18 +771,66 @@ SSX_planeInfo wd_planeLocalization(
}
//取左右点集的最低点，作为左右发动机的参考点
SVzNL3DPosition leftEnginePoint = distanceValidData[leftEngineIdx][0];
SVzNL3DPosition leftNearestPoint = 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];
if (leftNearestPoint.pt3D.z > distanceValidData[leftEngineIdx][i].pt3D.z)
leftNearestPoint = distanceValidData[leftEngineIdx][i];
}
SVzNL3DPosition rightEnginePoint = distanceValidData[rightEngineIdx][0];
std::vector<SVzNL3DPosition> leftEengineData;
for (int i = 0; i < (int)distanceValidData[leftEngineIdx].size(); i++)
{
double zDiff = distanceValidData[leftEngineIdx][i].pt3D.z - leftNearestPoint.pt3D.z;
if (zDiff < 500.0) //0.5米范围内
leftEengineData.push_back(distanceValidData[leftEngineIdx][i]);
}
SVzNL3DPosition leftEnginePoint = leftEengineData[0];
SVzNL3DPosition leftCentroid = { 0, {0, 0, 0} };
for (int i = 1; i < (int)leftEengineData.size(); i++)
{
if (leftEnginePoint.pt3D.y < leftEengineData[i].pt3D.y)
leftEnginePoint = leftEengineData[i];
leftCentroid.pt3D.x += leftEengineData[i].pt3D.x;
leftCentroid.pt3D.y += leftEengineData[i].pt3D.y;
leftCentroid.pt3D.z += leftEengineData[i].pt3D.z;
}
int leftEngineDataSize = (int)leftEengineData.size();
leftCentroid.pt3D.x = leftCentroid.pt3D.x / leftEngineDataSize;
leftCentroid.pt3D.y = leftCentroid.pt3D.y / leftEngineDataSize;
leftCentroid.pt3D.z = leftCentroid.pt3D.z / leftEngineDataSize;
SVzNL3DPosition rightNearestPoint = 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 (rightNearestPoint.pt3D.z > distanceValidData[rightEngineIdx][i].pt3D.z)
rightNearestPoint = distanceValidData[rightEngineIdx][i];
}
std::vector<SVzNL3DPosition> rightEngineData;
for (int i = 0; i < (int)distanceValidData[rightEngineIdx].size(); i++)
{
double zDiff = distanceValidData[rightEngineIdx][i].pt3D.z - rightNearestPoint.pt3D.z;
if (zDiff < 500.0) //0.5米范围内
rightEngineData.push_back(distanceValidData[rightEngineIdx][i]);
}
SVzNL3DPosition rightEnginePoint = rightEngineData[0];
SVzNL3DPosition rightCentroid = { 0, {0, 0, 0} };
for (int i = 1; i < (int)rightEngineData.size(); i++)
{
if (rightEnginePoint.pt3D.y < rightEngineData[i].pt3D.y)
rightEnginePoint = rightEngineData[i];
rightCentroid.pt3D.x += rightEngineData[i].pt3D.x;
rightCentroid.pt3D.y += rightEngineData[i].pt3D.y;
rightCentroid.pt3D.z += rightEngineData[i].pt3D.z;
}
int rightEngineDataSize = (int)rightEngineData.size();
rightCentroid.pt3D.x = rightCentroid.pt3D.x / rightEngineDataSize;
rightCentroid.pt3D.y = rightCentroid.pt3D.y / rightEngineDataSize;
rightCentroid.pt3D.z = rightCentroid.pt3D.z / rightEngineDataSize;
#if _OUTPUT_DEBUG_DATA
{
int nose_lineIdx = nosePoint.nPointIdx >> 16;
@ -794,6 +847,9 @@ SSX_planeInfo wd_planeLocalization(
debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x40000; //机鼻点
}
#endif
leftEnginePoint.pt3D = leftCentroid.pt3D;
rightEnginePoint.pt3D = rightCentroid.pt3D;
//计算姿态
//axis与左右发动机连续垂直, 采用(-y,, x)形式
SVzNL2DPointD axis = { -(rightEnginePoint.pt3D.z - leftEnginePoint.pt3D.z), rightEnginePoint.pt3D.x - leftEnginePoint.pt3D.x };