planeLocalization
version 1.2.0 : 修正了回归测试中发现的问题
This commit is contained in:
parent
a29a6c005c
commit
c31b4d35cf
@ -179,7 +179,15 @@ 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);
|
||||
sprintf_s(dataStr, 250, "距离: %g (mm)", planeInfo.distance);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "偏离引导线: %g (mm)", planeInfo.deviation);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "前进方向: %g (度)", planeInfo.dirAngle_deg);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "机鼻点: (%g , %g, %g)", planeInfo.nosePoint.x, planeInfo.nosePoint.y, planeInfo.nosePoint.z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "方向向量(轴向): (%g , %g, %g)", planeInfo.axis.x, planeInfo.axis.y, planeInfo.axis.z);
|
||||
sw << dataStr << std::endl;
|
||||
sw.close();
|
||||
}
|
||||
@ -249,15 +257,19 @@ void _outputRGBDScan_RGBD(
|
||||
sw.close();
|
||||
}
|
||||
|
||||
void _outputRGBDScan_superCluster(
|
||||
void _outputRGBDScan_superCluster_result(
|
||||
char* fileName,
|
||||
std::vector<std::vector<SVzNL3DPosition>>& scanLines
|
||||
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
|
||||
SSX_planeInfo& planePose
|
||||
)
|
||||
{
|
||||
int lineNum = (int)scanLines.size();
|
||||
std::ofstream sw(fileName);
|
||||
int realLines = lineNum;
|
||||
|
||||
if (planePose.nosePoint.z > 1e-4)
|
||||
realLines += 1;
|
||||
|
||||
sw << "LineNum:" << realLines << std::endl;
|
||||
sw << "DataType: 0" << std::endl;
|
||||
sw << "ScanSpeed: 0" << std::endl;
|
||||
@ -297,7 +309,7 @@ void _outputRGBDScan_superCluster(
|
||||
if (flag == 1) //机鼻
|
||||
{
|
||||
rgb = { 255, 0, 0 };
|
||||
size = 10;
|
||||
size = 20;
|
||||
}
|
||||
else if(flag == 2) //机身
|
||||
{
|
||||
@ -327,6 +339,40 @@ void _outputRGBDScan_superCluster(
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
}
|
||||
if (planePose.nosePoint.z > 1e-4)
|
||||
{
|
||||
sw << "Line_" << lineIdx << "_0_1" << std::endl;
|
||||
lineIdx++;
|
||||
rgb = { 255, 0, 0 };
|
||||
size = 20;
|
||||
float x = (float)planePose.nosePoint.x;
|
||||
float y = (float)planePose.nosePoint.y;
|
||||
float z = (float)planePose.nosePoint.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
//输出姿态
|
||||
//输出方向线条
|
||||
rgb = { 0, 0, 255 };
|
||||
size = 5;
|
||||
SVzNL3DPoint pt1, pt2;
|
||||
pt1 = { planePose.nosePoint.x - planePose.axis.x * 2000.0,
|
||||
planePose.nosePoint.y - planePose.axis.y * 2000.0,
|
||||
planePose.nosePoint.z - planePose.axis.z * 2000.0 };
|
||||
pt2 = { planePose.nosePoint.x + planePose.axis.x * 40000.0,
|
||||
planePose.nosePoint.y + planePose.axis.y * 40000.0,
|
||||
planePose.nosePoint.z + planePose.axis.z * 40000.0 };
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << pt1.x << "," << pt1.y << "," << pt1.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << pt2.x << "," << pt2.y << "," << pt2.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
lineIdx++;
|
||||
|
||||
}
|
||||
|
||||
sw.close();
|
||||
}
|
||||
|
||||
@ -587,7 +633,7 @@ int main()
|
||||
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx =18;
|
||||
//fidx =7;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
|
||||
|
||||
@ -623,16 +669,16 @@ int main()
|
||||
& errCode);
|
||||
|
||||
long t2 = (long)GetTickCount64();
|
||||
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
|
||||
printf("%s: %d(ms), errCode=%d ...", _scan_file, (int)(t2 - t1), errCode);
|
||||
//输出测试结果
|
||||
#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);
|
||||
_outputRGBDScan_superCluster_result(_scan_file, debugData, planeInfo);
|
||||
#endif
|
||||
sprintf_s(_scan_file, "%sresult\\%d_planePise_info.txt", dataPath[grp], fidx);
|
||||
_outputPlaneInfo(_scan_file, planeInfo);
|
||||
printf("done\n");
|
||||
printf(" distance=%f, devi=%f, angle=%f\n", planeInfo.distance, planeInfo.deviation, planeInfo.dirAngle_deg);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -6,7 +6,9 @@
|
||||
#include <limits>
|
||||
|
||||
//version 1.0.0 : base version release to customer
|
||||
std::string m_strVersion = " PlaneLocalization 1.0.0";
|
||||
//version 1.1.0 : 优化了机鼻点提取（迭代），增加了没有飞机的输出
|
||||
//version 1.2.0 : 修正了回归测试中发现的问题
|
||||
std::string m_strVersion = " PlaneLocalization 1.2.0";
|
||||
const char* wd_PlaneLocalizationVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -143,7 +145,7 @@ void _searchSuperCluster(
|
||||
a_superCluster.push_back(i);
|
||||
clusterFlags[i] = 1;
|
||||
}
|
||||
else if (score > 0)
|
||||
else //if (score > 0)
|
||||
{
|
||||
//计算距离:使用Z距离
|
||||
SVzNL3DRangeD& chkROI = clusterROIs[i];
|
||||
@ -215,6 +217,48 @@ SVzNL3DRangeD _getSupcluster(std::vector<int>& superClusterIndice, std::vector<S
|
||||
}
|
||||
return mergeROI;
|
||||
}
|
||||
|
||||
//判断聚类的最近点是否可能是机鼻点。方法：以最近点搜索在距离范围内的点（指向机身），计算机身高度
|
||||
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];
|
||||
}
|
||||
|
||||
//判断是否是机鼻点
|
||||
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;
|
||||
}
|
||||
|
||||
SSX_planeInfo wd_planeLocalization(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SSG_planeCalibPara groundCalibParam,
|
||||
@ -228,7 +272,7 @@ SSX_planeInfo wd_planeLocalization(
|
||||
*errCode = 0;
|
||||
//内部参数
|
||||
double planeMinHeight = 4000; //最小高度4米
|
||||
double planeMinWidth = 25000; //最小宽度2.5米
|
||||
double planeMinWidth = 2500; //最小宽度2.5米
|
||||
double nearFarTh = 80000.0; //远近分界
|
||||
double groundHOffset = 200; //去除地面参数
|
||||
double bodyHeadJudge_lenTh = 20000.0; //机头与机身判断的长度门限。20米
|
||||
@ -236,6 +280,8 @@ SSX_planeInfo wd_planeLocalization(
|
||||
SVzNLRangeD sameRBodyRange = { 5000.0, 28000.0 }; //机身圆桶段，以距离机鼻距离为基准
|
||||
SVzNLRangeD engineToNoseDistRange = { 12000.0, 16000.0 };
|
||||
SVzNLRangeD bodyYRange = { groundCalibParam.planeHeight - 4500.0, groundCalibParam.planeHeight - 1500 }; //机身的Y范围
|
||||
SVzNLRangeD nearestPointNostChkDistRange = { 3500.0, 4500.0 };
|
||||
double minBodyHeight = 2000.0;
|
||||
|
||||
SSX_planeInfo planePoseInfo;
|
||||
memset(&planePoseInfo, 0, sizeof(SSX_planeInfo));
|
||||
@ -527,16 +573,34 @@ SSX_planeInfo wd_planeLocalization(
|
||||
int planeClusterSize = (int)planeSuperCluster.size();
|
||||
//寻找机头位置: ROI最靠前（Z最小）
|
||||
int noseClusterId = -1;
|
||||
SVzNL3DPosition nosePoint;
|
||||
nosePoint.nPointIdx = 0;
|
||||
nosePoint.pt3D = { 0.0, 0.0, 0.0 };
|
||||
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;
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (noseClusterId < 0)
|
||||
@ -544,29 +608,7 @@ SSX_planeInfo wd_planeLocalization(
|
||||
*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 };
|
||||
@ -603,6 +645,24 @@ SSX_planeInfo wd_planeLocalization(
|
||||
if (axis.y < 0)
|
||||
axis = { -axis.x, -axis.y };
|
||||
|
||||
//以centrod为基准，计算到centroid最远的点为机鼻点
|
||||
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) //迭代计算机鼻点
|
||||
nosePoint = bestPoint;
|
||||
|
||||
double dirAngle = atan(axis.x / axis.y) * 180.0 / PI;
|
||||
planePoseInfo.nosePoint = nosePoint.pt3D;
|
||||
planePoseInfo.axis = { axis.x, 0, axis.y };
|
||||
@ -612,6 +672,11 @@ SSX_planeInfo wd_planeLocalization(
|
||||
planePoseInfo.dirAngle_deg = dirAngle; //方向角度
|
||||
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
{
|
||||
int nose_lineIdx = nosePoint.nPointIdx >> 16;
|
||||
int nose_ptIdx = nosePoint.nPointIdx & 0xffff;
|
||||
debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x10000; //机鼻点
|
||||
}
|
||||
for(int m = 0; m <(int)XOZBodayData.size(); m ++)
|
||||
{
|
||||
int nose_lineIdx = XOZBodayData[m].nPointIdx >> 16;
|
||||
@ -710,6 +775,11 @@ SSX_planeInfo wd_planeLocalization(
|
||||
rightEnginePoint = distanceValidData[rightEngineIdx][i];
|
||||
}
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
{
|
||||
int nose_lineIdx = nosePoint.nPointIdx >> 16;
|
||||
int nose_ptIdx = nosePoint.nPointIdx & 0xffff;
|
||||
debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x10000; //机鼻点
|
||||
}
|
||||
{
|
||||
int nose_lineIdx = leftEnginePoint.nPointIdx >> 16;
|
||||
int nose_ptIdx = leftEnginePoint.nPointIdx & 0xffff;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user