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

70
sourceCode/planeLocalization.cpp
View File

@ -9,7 +9,8 @@
//version 1.1.0 : 优化了机鼻点提取（迭代），增加了没有飞机的输出 //version 1.1.0 : 优化了机鼻点提取（迭代），增加了没有飞机的输出
//version 1.2.0 : 修正了回归测试中发现的问题 //version 1.2.0 : 修正了回归测试中发现的问题
//version 1.3.0 : 修正了取发动机点特征点的一个Bug //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) const char* wd_PlaneLocalizationVersion(void)
{ {
return m_strVersion.c_str(); return m_strVersion.c_str();
@ -432,6 +433,9 @@ SSX_planeInfo wd_planeLocalization(
for (int m = 0; m < (int)allClusters.size(); m++) for (int m = 0; m < (int)allClusters.size(); m++)
{ {
SVzNL3DRangeD& a_roi = allClusterROIs[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.zRange.min - rotateParkingPoint.z) < nearFarTh)
{ {
if ((a_roi.xRange.min >= ROI_x_near.min) && (a_roi.xRange.max <= ROI_x_near.max) && 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 //建立聚类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++) for (int i = 1; i < (int)distanceValidData[leftEngineIdx].size(); i++)
{ {
if (leftEnginePoint.pt3D.y < distanceValidData[leftEngineIdx][i].pt3D.y) if (leftNearestPoint.pt3D.z > distanceValidData[leftEngineIdx][i].pt3D.z)
leftEnginePoint = distanceValidData[leftEngineIdx][i]; 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++) for (int i = 1; i < (int)distanceValidData[rightEngineIdx].size(); i++)
{ {
if (rightEnginePoint.pt3D.y < distanceValidData[rightEngineIdx][i].pt3D.y) if (rightNearestPoint.pt3D.z > distanceValidData[rightEngineIdx][i].pt3D.z)
rightEnginePoint = distanceValidData[rightEngineIdx][i]; 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 #if _OUTPUT_DEBUG_DATA
{ {
int nose_lineIdx = nosePoint.nPointIdx >> 16; int nose_lineIdx = nosePoint.nPointIdx >> 16;
@ -794,6 +847,9 @@ SSX_planeInfo wd_planeLocalization(
debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x40000; //机鼻点 debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x40000; //机鼻点
} }
#endif #endif
leftEnginePoint.pt3D = leftCentroid.pt3D;
rightEnginePoint.pt3D = rightCentroid.pt3D;
//计算姿态 //计算姿态
//axis与左右发动机连续垂直, 采用(-y,, x)形式 //axis与左右发动机连续垂直, 采用(-y,, x)形式
SVzNL2DPointD axis = { -(rightEnginePoint.pt3D.z - leftEnginePoint.pt3D.z), rightEnginePoint.pt3D.x - leftEnginePoint.pt3D.x }; SVzNL2DPointD axis = { -(rightEnginePoint.pt3D.z - leftEnginePoint.pt3D.z), rightEnginePoint.pt3D.x - leftEnginePoint.pt3D.x };