741 lines
24 KiB
C++
741 lines
24 KiB
C++
#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;
|
||
}
|
||
|