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

933 lines
30 KiB
C++
Raw Normal View History

planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
#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
planeLocalization version 1.2.0 : 修正了回归测试中发现的问题
2026-07-17 19:11:38 +08:00
//version 1.1.0 : <20>Ż<EFBFBD><C5BB>˻<EFBFBD><CBBB>ǵ<EFBFBD><C7B5><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD>зɻ<D0B7><C9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//version 1.2.0 : <20><><EFBFBD><EFBFBD><EFBFBD>˻ع<CBBB><D8B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>з<EFBFBD><D0B7>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD>
planeLocalization version 1.3.0 : 修正了取发动机点特征点的一个Bug
2026-07-17 19:36:08 +08:00
//version 1.3.0 : <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>Bug
planeLocalization version 1.3.1 : 改进了取发动机点特征点算法
2026-07-29 23:56:59 +08:00
//version 1.3.1 : <20>Ľ<EFBFBD><C4BD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨
planeLocalization version 1.3.2 : 改进了取发动机点特征点算法, 增强了鲁棒性
2026-07-30 11:01:35 +08:00
//version 1.3.2 : <20>Ľ<EFBFBD><C4BD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨, <20><>ǿ<EFBFBD><C7BF>³<EFBFBD><C2B3><EFBFBD><EFBFBD>
std::string m_strVersion = " PlaneLocalization 1.3.2";
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
const char* wd_PlaneLocalizationVersion(void)
{
return m_strVersion.c_str();
}
//ʹ<><CAB9>RANSAC<41><43><EFBFBD><EFBFBD><EFBFBD>Ƽ<EFBFBD><C6BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
SSG_planeCalibPara wd_getGroundCalibPara(
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
//<2F><><EFBFBD>ó<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>
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<30><30><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD><EFBFBD><EFBFBD>
{
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);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߶<EFBFBD>
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];
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
}
planeLocalization version 1.2.0 : 修正了回归测试中发现的问题
2026-07-17 19:11:38 +08:00
else //if (score > 0)
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:ʹ<><CAB9>Z<EFBFBD><5A><EFBFBD><EFBFBD>
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; //<2F>ص<EFBFBD>
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;
}
planeLocalization version 1.2.0 : 修正了回归测试中发现的问题
2026-07-17 19:11:38 +08:00
//<2F>жϾ<D0B6><CFBE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǻ<EFBFBD><C7BB>ǵ㡣<C7B5><E3A1A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ھ<EFBFBD><DABE>뷶Χ<EBB7B6>ڵĵ㣨ָ<E3A3A8><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߶<EFBFBD>
bool _checkNosePoint(
std::vector<SVzNL3DPosition>& a_cluster,
SVzNL3DPosition& nearestPoint,
SVzNLRangeD& distChkRange,
const double bodyHeight)
{
nearestPoint.nPointIdx = 0;
nearestPoint.pt3D = { 0.0, 0.0, 0.0 };
for (int i = 0; i < (int)a_cluster.size(); i++)
{
if (nearestPoint.pt3D.z < 1e-4)
nearestPoint = a_cluster[i];
else if (nearestPoint.pt3D.z > a_cluster[i].pt3D.z)
nearestPoint = a_cluster[i];
}
//<2F>ж<EFBFBD><D0B6>Ƿ<EFBFBD><C7B7>ǻ<EFBFBD><C7BB>ǵ<EFBFBD>
std::vector<SVzNL3DPoint> chkPoints;
for (int i = 0; i < (int)a_cluster.size(); i++)
{
double dist = sqrt(pow(a_cluster[i].pt3D.x - nearestPoint.pt3D.x, 2) + pow(a_cluster[i].pt3D.z - nearestPoint.pt3D.z, 2));
if ((dist >= distChkRange.min) && (dist <= distChkRange.max))
chkPoints.push_back(a_cluster[i].pt3D);
}
if (chkPoints.size() < 2)
return false;
SVzNLRangeD yRange = { chkPoints[0].y, chkPoints[0].y };
for (int i = 1; i < (int)chkPoints.size(); i++)
{
yRange.min = yRange.min > chkPoints[i].y ? chkPoints[i].y : yRange.min;
yRange.max = yRange.max < chkPoints[i].y ? chkPoints[i].y : yRange.max;
}
double height = yRange.max - yRange.min;
if (height > bodyHeight)
return true;
return false;
}
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
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;
//<2F>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>
double planeMinHeight = 4000; //<2F><>С<EFBFBD>߶<EFBFBD>4<EFBFBD><34>
planeLocalization version 1.2.0 : 修正了回归测试中发现的问题
2026-07-17 19:11:38 +08:00
double planeMinWidth = 2500; //<2F><>С<EFBFBD><D0A1><EFBFBD><EFBFBD>2.5<EFBFBD><EFBFBD>
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
double nearFarTh = 80000.0; //Զ<><D4B6><EFBFBD>ֽ<EFBFBD>
double groundHOffset = 200; //ȥ<><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double bodyHeadJudge_lenTh = 20000.0; //<2F><>ͷ<EFBFBD><CDB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϵij<CFB5><C4B3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ޡ<EFBFBD>20<32><30>
SVzNLRangeD bodyRangeToNose = { -2000.0, 1000.0 }; //<2F>Ի<EFBFBD><D4BB>Ǹ߶<C7B8>Ϊ<EFBFBD><CEAA>׼<EFBFBD><D7BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĸ߶ȷ<DFB6>Χ
SVzNLRangeD sameRBodyRange = { 5000.0, 28000.0 }; //<2F><><EFBFBD><EFBFBD>ԲͰ<D4B2>Σ<EFBFBD><CEA3>Ծ<EFBFBD><D4BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǿ<EFBFBD><C7BE><EFBFBD>Ϊ<EFBFBD><CEAA>׼
SVzNLRangeD engineToNoseDistRange = { 12000.0, 16000.0 };
SVzNLRangeD bodyYRange = { groundCalibParam.planeHeight - 4500.0, groundCalibParam.planeHeight - 1500 }; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Y<EFBFBD><59>Χ
planeLocalization version 1.2.0 : 修正了回归测试中发现的问题
2026-07-17 19:11:38 +08:00
SVzNLRangeD nearestPointNostChkDistRange = { 3500.0, 4500.0 };
double minBodyHeight = 2000.0;
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
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();
//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ݸ<EFBFBD>ʽ<EFBFBD>Ƿ<EFBFBD>Ϊgrid<69><64><EFBFBD>㷨ֻ<E3B7A8>ܴ<EFBFBD><DCB4><EFBFBD>grid<69><64><EFBFBD>ݸ<EFBFBD>ʽ
bool isGridData = true;
for (int line = 0; line < lineNum; line++)
{
if (linePtNum != (int)scanLines[line].size())
{
isGridData = false;
break;
}
}
if (false == isGridData)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return planePoseInfo;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>Y<EFBFBD><59>
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);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
SVzNL3DPoint rotateParkingPoint = wd_ptRotate(parkingParam.parkingPoint, guideLineRotatePara.planeCalib);
SVzNL3DPoint chkPoint = wd_ptRotate(parkingParam.guideLinePoint, guideLineRotatePara.planeCalib);
//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD>귶Χ
planeLocalization version 1.3.2 : 改进了取发动机点特征点算法, 增强了鲁棒性
2026-07-30 11:01:35 +08:00
SVzNLRangeD ROI_x_validPlaneInRng = { rotateParkingPoint.x - 5000.0, rotateParkingPoint.x + 5000.0 };
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
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 }; //<2F><><EFBFBD><EFBFBD>100m<30>⣬<EFBFBD><E2A3AC>Ҫ<EFBFBD>ſ<EFBFBD><C5BF><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>ɻ<EFBFBD><C9BB><EFBFBD><EFBFBD>ڽ<EFBFBD><DABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
SVzNLRangeD ROI_z = { rotateParkingPoint.z - parkingParam.distFromNoseToWheel - 2000.0, rotateParkingPoint.z + parkingParam.guidingRange };
//<2F><>ת<EFBFBD><D7AA> ROI<4F><49><EFBFBD><EFBFBD>
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 };
//<2F>ٴ<EFBFBD><D9B4><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>Y<EFBFBD><59>
a_pt.pt3D = wd_ptRotate(a_pt.pt3D, guideLineRotatePara.planeCalib);
rotateROIData[line][j] = a_pt;
}
}
std::vector< std::vector<SWD_poloarScan2D>> polarScanData; //ɨ<><C9A8><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD>ɨ<EFBFBD><C9A8>ˮƽ<CBAE>ǡ<EFBFBD><C7A1><EFBFBD>ֱ<EFBFBD>Ǻ;<C7BA><CDBE><EFBFBD>,
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) //ȥ<><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
SVzNL3DPosition a_pt;
a_pt.nPointIdx = (line << 16) | (j & 0xffff);
a_pt.pt3D = rotateROIData[line][j].pt3D;
validPoints.push_back(a_pt);
//<2F><><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>Ƕȣ<C7B6>ʹ<EFBFBD><CAB9><EFBFBD><EFBFBD>תǰ<D7AA><C7B0><EFBFBD><EFBFBD>
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));
//ˮƽ<CBAE>ǣ<EFBFBD>
double yaw_rad = atan2(-a_pt.pt3D.x, a_pt.pt3D.z);
a_polarData.yaw_deg = yaw_rad * 180 / PI;
//<2F><>ֱ<EFBFBD>ǣ<EFBFBD>
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
//<2F><><EFBFBD><EFBFBD>ͶӰ<CDB6><D3B0><EFBFBD><EFBFBD>
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
//<2F><><EFBFBD><EFBFBD>
//ʹ<><CAB9>SVzNL3DPosition<6F><6E>nPointIdx<64><78>ʾ2D<32><44>Ϣ<EFBFBD><CFA2><EFBFBD><EFBFBD>16λLine<6E><65> <20><>16λptIdx<64><78>
//<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><D4BC><EFBFBD>
int clusterCheckWin = 5;
planeLocalization version 1.3.2 : 改进了取发动机点特征点算法, 增强了鲁棒性
2026-07-30 11:01:35 +08:00
double clusterDist = 500; // sqrt(pow(growParam.zDeviation_max, 2) + pow(growParam.maxSkipDistance, 2) + pow(growParam.yDeviation_max, 2));
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
int distType = 1; //0 - 2d distance; 1- 3d distance
std::vector<std::vector< SVzNL3DPosition>> allClusters;
wd_pointClustering_speedUp(
validPoints,
lineNum, linePtNum, clusterCheckWin, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
clusterDist,
distType, //0 - 2d distance; 1- 3d distance
allClusters //result
);
//ͳ<><CDB3>cluster<65><72>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;
}
planeLocalization version 1.3.2 : 改进了取发动机点特征点算法, 增强了鲁棒性
2026-07-30 11:01:35 +08:00
#if 1
//<2F><>СROI<4F><49>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD><CBB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD>ô<EFBFBD><C3B4>߶Ƚ<DFB6><C8BD>о<EFBFBD><D0BE><EFBFBD>
validPoints.clear();
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<>׸߶<D7B8><DFB6><EFBFBD><EFBFBD>ޣ<EFBFBD> <20><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
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))
{
validPoints.insert(validPoints.end(), allClusters[m].begin(), allClusters[m].end());
}
}
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))
{
validPoints.insert(validPoints.end(), allClusters[m].begin(), allClusters[m].end());
}
}
}
}
//<2F><><EFBFBD><EFBFBD>
//ʹ<><CAB9>SVzNL3DPosition<6F><6E>nPointIdx<64><78>ʾ2D<32><44>Ϣ<EFBFBD><CFA2><EFBFBD><EFBFBD>16λLine<6E><65> <20><>16λptIdx<64><78>
//<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><D4BC><EFBFBD>
clusterCheckWin = 5;
clusterDist = sqrt(pow(growParam.zDeviation_max, 2) + pow(growParam.maxSkipDistance, 2) + pow(growParam.yDeviation_max, 2));
distType = 1; //0 - 2d distance; 1- 3d distance
allClusters.clear();
wd_pointClustering_speedUp(
validPoints,
lineNum, linePtNum, clusterCheckWin, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
clusterDist,
distType, //0 - 2d distance; 1- 3d distance
allClusters //result
);
//ͳ<><CDB3>cluster<65><72>ROI
allClusterROIs.clear();
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;
}
#endif
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
//ROI<4F><49><EFBFBD><EFBFBD>
std::vector<std::vector< SVzNL3DPosition>> objClusters;
std::vector<SVzNL3DRangeD> objClustersROIs;
for (int m = 0; m < (int)allClusters.size(); m++)
{
SVzNL3DRangeD& a_roi = allClusterROIs[m];
planeLocalization version 1.3.1 : 改进了取发动机点特征点算法
2026-07-29 23:56:59 +08:00
double height = groundCalibParam.planeHeight - a_roi.yRange.min;
if (height >= 2000.00) //2<>׸߶<D7B8><DFB6><EFBFBD><EFBFBD>ޣ<EFBFBD> <20><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
{
planeLocalization version 1.3.1 : 改进了取发动机点特征点算法
2026-07-29 23:56:59 +08:00
if ((a_roi.zRange.min - rotateParkingPoint.z) < nearFarTh)
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
{
planeLocalization version 1.3.1 : 改进了取发动机点特征点算法
2026-07-29 23:56:59 +08:00
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);
}
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
}
planeLocalization version 1.3.1 : 改进了取发动机点特征点算法
2026-07-29 23:56:59 +08:00
else
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
{
planeLocalization version 1.3.1 : 改进了取发动机点特征点算法
2026-07-29 23:56:59 +08:00
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);
}
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
}
}
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֮<EFBFBD><D6AE><EFBFBD>Ĺ<EFBFBD>ϵ
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>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;
}
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڹ<EFBFBD>ϵ<EFBFBD><CFB5>
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;
}
}
//<2F><><EFBFBD>ξ<EFBFBD><CEBE><EFBFBD><EFBFBD>ɳ<EFBFBD><C9B3><EFBFBD><EFBFBD><EFBFBD>
int adjacencyScoreTh = 10;
double adjacencyZDistTh = 18000.0;//18<31>׳<EFBFBD>
std::vector<std::vector<int>> superClusers; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>࣬ʹ<E0A3AC>þ<EFBFBD><C3BE><EFBFBD><EFBFBD><EFBFBD>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);
//<2F>ݹ<EFBFBD><DDB9><EFBFBD><EFBFBD><EFBFBD>
_searchSuperCluster(a_superCluster, adjacencyScoreTable, clusterFlags, objClustersROIs, adjacencyScoreTh, adjacencyZDistTh);
//<2F><><EFBFBD><EFBFBD>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;
planeLocalization version 1.3.2 : 改进了取发动机点特征点算法, 增强了鲁棒性
2026-07-30 11:01:35 +08:00
if( (h > planeMinHeight) && (w > planeMinWidth) &&
(superROI.xRange.max > ROI_x_validPlaneInRng.min) && (ROI_x_validPlaneInRng.max > superROI.xRange.min))
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
superClusers.push_back(a_superCluster);
}
#if _OUTPUT_DEBUG_DATA
//<2F><>ע
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;
}
//<2F><>ѡ<EFBFBD><D1A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ij<EFBFBD><C4B3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>ɻ<EFBFBD>
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];
planeLocalization version 1.3.2 : 改进了取发动机点特征点算法, 增强了鲁棒性
2026-07-30 11:01:35 +08:00
//<2F><><EFBFBD><EFBFBD>ROI
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
int planeClusterSize = (int)planeSuperCluster.size();
//Ѱ<>һ<EFBFBD>ͷλ<CDB7><CEBB>: ROI<4F>ǰ<EEBFBF><C7B0>Z<EFBFBD><5A>С<EFBFBD><D0A1>
int noseClusterId = -1;
planeLocalization version 1.2.0 : 修正了回归测试中发现的问题
2026-07-17 19:11:38 +08:00
SVzNL3DPosition nosePoint;
nosePoint.nPointIdx = 0;
nosePoint.pt3D = { 0.0, 0.0, 0.0 };
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
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)) //<2F><><EFBFBD>ڻ<EFBFBD><DABB><EFBFBD><EFBFBD><EFBFBD>Y<EFBFBD><59>Χ<EFBFBD><CEA7>
{
planeLocalization version 1.2.0 : 修正了回归测试中发现的问题
2026-07-17 19:11:38 +08:00
SVzNL3DPosition nearestPoint;
bool validNoseCluster = _checkNosePoint(
objClusters[clusterId],
nearestPoint,
nearestPointNostChkDistRange,
minBodyHeight);
if (true == validNoseCluster)
{
if (noseClusterId < 0)
{
noseClusterId = clusterId;
nosePoint = nearestPoint;
}
else if (nosePoint.pt3D.z > nearestPoint.pt3D.z)
{
noseClusterId = clusterId;
nosePoint = nearestPoint;
}
}
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
}
}
if (noseClusterId < 0)
{
*errCode = SX_ERR_NOSEPOINT_FAIL;
return planePoseInfo;
}
planeLocalization version 1.2.0 : 修正了回归测试中发现的问题
2026-07-17 19:11:38 +08:00
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬
//<2F>жϻ<D0B6><CFBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵij<DAB5><C4B3>ȣ<EFBFBD><C8A3>Ի<EFBFBD><D4BB>Ǹ߶<C7B8>Ϊ<EFBFBD><CEAA>׼<EFBFBD><D7BC>ȡ<EFBFBD>߶ȷ<DFB6>Χ<EFBFBD>ڵĵ㣬<C4B5><E3A3AC><EFBFBD>㳤<EFBFBD><E3B3A4>
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]);
}
//<2F><><EFBFBD>㵽<EFBFBD><E3B5BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EEB3A4><EFBFBD><EFBFBD>
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) //<2F><><EFBFBD><EFBFBD>
{
//ȡ<><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD>PCA<43><41><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬
// <20><>άPCA<43><41><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ʒ<EFBFBD><C6B7><EFBFBD>
SVzNL2DPointD axis, centroid;
pca2D(XOZBodayProjectionData, axis, centroid);
if (axis.y < 0)
axis = { -axis.x, -axis.y };
planeLocalization version 1.2.0 : 修正了回归测试中发现的问题
2026-07-17 19:11:38 +08:00
//<2F><>centrodΪ<64><CEAA>׼<EFBFBD><D7BC><EFBFBD><EFBFBD><EFBFBD>㵽centroid<69><64>Զ<EFBFBD>ĵ<EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>ǵ<EFBFBD>
double maxDistance = 0;
SVzNL3DPosition bestPoint;
for (int m = 0; m < (int)objClusters[noseClusterId].size(); m++)
{
if (objClusters[noseClusterId][m].pt3D.z < centroid.y)
{
double dist = sqrt(pow(objClusters[noseClusterId][m].pt3D.x - centroid.x, 2) + pow(objClusters[noseClusterId][m].pt3D.z - centroid.y, 2));
if (maxDistance < dist)
{
maxDistance = dist;
bestPoint = objClusters[noseClusterId][m];
}
}
}
if(maxDistance > 1e-4) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD>
nosePoint = bestPoint;
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
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; //ƫ<><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
planePoseInfo.dirAngle_deg = dirAngle; //<2F><><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>
#if _OUTPUT_DEBUG_DATA
planeLocalization version 1.2.0 : 修正了回归测试中发现的问题
2026-07-17 19:11:38 +08:00
{
int nose_lineIdx = nosePoint.nPointIdx >> 16;
int nose_ptIdx = nosePoint.nPointIdx & 0xffff;
debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x10000; //<2F><><EFBFBD>ǵ<EFBFBD>
}
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
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; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
}
#endif
}
else //<2F><>ͷ
{
//ȡ<><C8A1><EFBFBD>ҷ<EFBFBD><D2B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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++)
{
planeLocalization version 1.3.0 : 修正了取发动机点特征点的一个Bug
2026-07-17 19:36:08 +08:00
if (objClusters[clusterIdx][i].pt3D.z > nosePoint.pt3D.z)
{
double dist = sqrt(pow(nosePoint.pt3D.x - objClusters[clusterIdx][i].pt3D.x, 2) + pow(nosePoint.pt3D.z - objClusters[clusterIdx][i].pt3D.z, 2));
planeLocalization version 1.3.2 : 改进了取发动机点特征点算法, 增强了鲁棒性
2026-07-30 11:01:35 +08:00
double zDiff = objClusters[clusterIdx][i].pt3D.z - nosePoint.pt3D.z;
if ((dist >= engineToNoseDistRange.min) && (dist <= engineToNoseDistRange.max) && (zDiff > 10000.0)) //<2F>ڻ<EFBFBD><DABB>Ǻ<EFBFBD><C7BA><EFBFBD>10<31><30><EFBFBD><EFBFBD>
planeLocalization version 1.3.0 : 修正了取发动机点特征点的一个Bug
2026-07-17 19:36:08 +08:00
distanceValidData[idx].push_back(objClusters[clusterIdx][i]);
}
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
}
}
//<2F><><EFBFBD><EFBFBD>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;
}
//ȡ<><C8A1><EFBFBD><EFBFBD>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;
}
//ȡ<><C8A1><EFBFBD>ҵ㼯<D2B5><E3BCAF><EFBFBD><EFBFBD><EFBFBD>͵㣬<CDB5><E3A3AC>Ϊ<EFBFBD><CEAA><EFBFBD>ҷ<EFBFBD><D2B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>IJο<C4B2><CEBF><EFBFBD>
planeLocalization version 1.3.1 : 改进了取发动机点特征点算法
2026-07-29 23:56:59 +08:00
SVzNL3DPosition leftNearestPoint = distanceValidData[leftEngineIdx][0];
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
for (int i = 1; i < (int)distanceValidData[leftEngineIdx].size(); i++)
{
planeLocalization version 1.3.1 : 改进了取发动机点特征点算法
2026-07-29 23:56:59 +08:00
if (leftNearestPoint.pt3D.z > distanceValidData[leftEngineIdx][i].pt3D.z)
leftNearestPoint = distanceValidData[leftEngineIdx][i];
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
}
planeLocalization version 1.3.1 : 改进了取发动机点特征点算法
2026-07-29 23:56:59 +08:00
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<EFBFBD>׷<EFBFBD>Χ<EFBFBD><EFBFBD>
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];
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
for (int i = 1; i < (int)distanceValidData[rightEngineIdx].size(); i++)
{
planeLocalization version 1.3.1 : 改进了取发动机点特征点算法
2026-07-29 23:56:59 +08:00
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<EFBFBD>׷<EFBFBD>Χ<EFBFBD><EFBFBD>
rightEngineData.push_back(distanceValidData[rightEngineIdx][i]);
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
}
planeLocalization version 1.3.1 : 改进了取发动机点特征点算法
2026-07-29 23:56:59 +08:00
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;
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
#if _OUTPUT_DEBUG_DATA
planeLocalization version 1.2.0 : 修正了回归测试中发现的问题
2026-07-17 19:11:38 +08:00
{
int nose_lineIdx = nosePoint.nPointIdx >> 16;
int nose_ptIdx = nosePoint.nPointIdx & 0xffff;
debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x10000; //<2F><><EFBFBD>ǵ<EFBFBD>
}
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
{
int nose_lineIdx = leftEnginePoint.nPointIdx >> 16;
int nose_ptIdx = leftEnginePoint.nPointIdx & 0xffff;
debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x40000; //<2F><><EFBFBD>ǵ<EFBFBD>
nose_lineIdx = rightEnginePoint.nPointIdx >> 16;
nose_ptIdx = rightEnginePoint.nPointIdx & 0xffff;
debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x40000; //<2F><><EFBFBD>ǵ<EFBFBD>
}
#endif
planeLocalization version 1.3.1 : 改进了取发动机点特征点算法
2026-07-29 23:56:59 +08:00
leftEnginePoint.pt3D = leftCentroid.pt3D;
rightEnginePoint.pt3D = rightCentroid.pt3D;
planeLocalization version 1.0.0 : base version release to customer
2026-07-17 17:05:44 +08:00
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬
//axis<69><73><EFBFBD><EFBFBD><EFBFBD>ҷ<EFBFBD><D2B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ, <20><><EFBFBD><EFBFBD>(-y,, x)<29><>ʽ
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; //ƫ<><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
planePoseInfo.dirAngle_deg = dirAngle; //<2F><><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>
}
return planePoseInfo;
}
Reference in New Issue Copy Permalink