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

369 lines
12 KiB
C++
Raw Normal View History

gasFillingPortPosition ver 1.0.0 加气口定位程序初始版本
2025-12-16 21:00:17 +08:00
#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "gasFillingPortPosition_Export.h"
#include <opencv2/opencv.hpp>
#include <limits>
//<2F><><EFBFBD>汾<EFBFBD><E6B1BE>
//version 1.0.0 : base version release to customer
gasFillingPortPosition ver 1.1.0 改进了加气口顶面点提取的准确度。
2025-12-16 23:09:30 +08:00
//version 1.1.0 : <20>Ľ<EFBFBD><C4BD><EFBFBD><EFBFBD>㷨<EFBFBD><E3B7A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1>׼ȷ
std::string m_strVersion = "1.1.0";
gasFillingPortPosition ver 1.0.0 加气口定位程序初始版本
2025-12-16 21:00:17 +08:00
const char* wd_gasFillingPortPositionVersion(void)
{
return m_strVersion.c_str();
}
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
SSG_6DOF wd_getGasFillingPortPosition(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSX_gasFillingPortPara gasFillingPortPara,
const SSG_lineSegParam lineSegPara,
const SSG_outlierFilterParam filterParam,
SSG_treeGrowParam growParam,
int* errCode)
{
*errCode = 0;
SSG_6DOF resultPose;
memset(&resultPose, 0, sizeof(SSG_6DOF));
int lineNum = (int)scanLines.size();
int linePtNum = (int)scanLines[0].size();
bool isGridData = true;
//<2F><>ȡֱ<C8A1>߶<EFBFBD><DFB6><EFBFBD><EFBFBD><EFBFBD>
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
SVzNLRangeD lineLenRange;
lineLenRange.max = 1.2 * (gasFillingPortPara.outerD - gasFillingPortPara.innerD);
lineLenRange.min = 0.5 * (gasFillingPortPara.outerD - gasFillingPortPara.innerD) / 2;
std::vector<std::vector<SSG_featureSemiCircle>> lineFeatures_v_raw;
gasFillingPortPosition ver 1.1.0 改进了加气口顶面点提取的准确度。
2025-12-16 23:09:30 +08:00
std::vector<std::vector<SSG_featureSemiCircle>> invalidFeatures_v_raw; //ȷ<><C8B7><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڷ<EFBFBD>ֹ<EFBFBD><D6B9>ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>
gasFillingPortPosition ver 1.0.0 加气口定位程序初始版本
2025-12-16 21:00:17 +08:00
for (int line = 0; line < lineNum; line++)
{
gasFillingPortPosition ver 1.1.0 改进了加气口顶面点提取的准确度。
2025-12-16 23:09:30 +08:00
if (line == 310)
gasFillingPortPosition ver 1.0.0 加气口定位程序初始版本
2025-12-16 21:00:17 +08:00
int kkk = 1;
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
if (linePtNum != (int)lineData.size())
isGridData = false;
//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
std::vector<SSG_featureSemiCircle> line_features;
gasFillingPortPosition ver 1.1.0 改进了加气口顶面点提取的准确度。
2025-12-16 23:09:30 +08:00
std::vector<SSG_featureSemiCircle> invalid_features;
gasFillingPortPosition ver 1.0.0 加气口定位程序初始版本
2025-12-16 21:00:17 +08:00
int dataSize = (int)lineData.size();
gasFillingPortPosition ver 1.1.0 改进了加气口顶面点提取的准确度。
2025-12-16 23:09:30 +08:00
wd_surfaceLineSegment(
gasFillingPortPosition ver 1.0.0 加气口定位程序初始版本
2025-12-16 21:00:17 +08:00
lineData,
line,
lineLenRange,
lineSegPara,
gasFillingPortPosition ver 1.1.0 改进了加气口顶面点提取的准确度。
2025-12-16 23:09:30 +08:00
line_features,
invalid_features);
gasFillingPortPosition ver 1.0.0 加气口定位程序初始版本
2025-12-16 21:00:17 +08:00
lineFeatures_v_raw.push_back(line_features);
gasFillingPortPosition ver 1.1.0 改进了加气口顶面点提取的准确度。
2025-12-16 23:09:30 +08:00
invalidFeatures_v_raw.push_back(invalid_features);
gasFillingPortPosition ver 1.0.0 加气口定位程序初始版本
2025-12-16 21:00:17 +08:00
}
if (false == isGridData)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return resultPose;
}
//<2F><><EFBFBD><EFBFBD>ˮƽɨ<C6BD><C9A8>
std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
hLines_raw.resize(linePtNum);
for (int i = 0; i < linePtNum; i++)
hLines_raw[i].resize(lineNum);
for (int line = 0; line < lineNum; line++)
{
for (int j = 0; j < linePtNum; j++)
{
scanLines[line][j].nPointIdx = 0; //<2F><>ԭʼ<D4AD><CABC><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ʹ<EFBFBD>ã<EFBFBD>
hLines_raw[j][line] = scanLines[line][j];
hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y;
hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x;
}
}
//ˮƽarc<72><63><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
std::vector<std::vector<SSG_featureSemiCircle>> lineFeatures_h_raw;
gasFillingPortPosition ver 1.1.0 改进了加气口顶面点提取的准确度。
2025-12-16 23:09:30 +08:00
std::vector<std::vector<SSG_featureSemiCircle>> invalidFeatures_h_raw; //ȷ<><C8B7><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڷ<EFBFBD>ֹ<EFBFBD>ڴ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>
gasFillingPortPosition ver 1.0.0 加气口定位程序初始版本
2025-12-16 21:00:17 +08:00
int lineNum_h_raw = (int)hLines_raw.size();
for (int line = 0; line < lineNum_h_raw; line++)
{
if (line == 974)
int kkk = 1;
std::vector<SVzNL3DPosition>& lineData = hLines_raw[line];
//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
int ptNum = (int)lineData.size();
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam);
std::vector<SSG_featureSemiCircle> line_features;
gasFillingPortPosition ver 1.1.0 改进了加气口顶面点提取的准确度。
2025-12-16 23:09:30 +08:00
std::vector<SSG_featureSemiCircle> invalid_features;
gasFillingPortPosition ver 1.0.0 加气口定位程序初始版本
2025-12-16 21:00:17 +08:00
int dataSize = (int)lineData.size();
gasFillingPortPosition ver 1.1.0 改进了加气口顶面点提取的准确度。
2025-12-16 23:09:30 +08:00
wd_surfaceLineSegment(
gasFillingPortPosition ver 1.0.0 加气口定位程序初始版本
2025-12-16 21:00:17 +08:00
lineData,
line,
lineLenRange,
lineSegPara,
gasFillingPortPosition ver 1.1.0 改进了加气口顶面点提取的准确度。
2025-12-16 23:09:30 +08:00
line_features,
invalid_features);
gasFillingPortPosition ver 1.0.0 加气口定位程序初始版本
2025-12-16 21:00:17 +08:00
lineFeatures_h_raw.push_back(line_features);
gasFillingPortPosition ver 1.1.0 改进了加气口顶面点提取的准确度。
2025-12-16 23:09:30 +08:00
invalidFeatures_h_raw.push_back(invalid_features);
gasFillingPortPosition ver 1.0.0 加气口定位程序初始版本
2025-12-16 21:00:17 +08:00
}
//<2F><>ע
std::vector<std::vector<SSG_featureClusteringInfo>> featureInfoMask;
std::vector<std::vector<SVzNL3DPoint>> feature3DInfo;
featureInfoMask.resize(lineNum);
feature3DInfo.resize(lineNum);
for (int i = 0; i < lineNum; i++)
{
featureInfoMask[i].resize(lineNum_h_raw);
feature3DInfo[i].resize(lineNum_h_raw);
}
//<2F><>ֱ<EFBFBD><D6B1>ע
for (int line = 0; line < lineNum; line++)
{
if (line == 390)
int kkk = 1;
std::vector<SSG_featureSemiCircle>& a_lineJumpFeature = lineFeatures_v_raw[line];
for (int m = 0, m_max = (int)a_lineJumpFeature.size(); m < m_max; m++)
{
int px = a_lineJumpFeature[m].lineIdx;
int py = a_lineJumpFeature[m].midPtIdx;
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[px][py];
a_featureInfo.clusterID = 0;
a_featureInfo.featurType = 1;
a_featureInfo.featureIdx_v = m;
a_featureInfo.featureIdx_h = 0;
a_featureInfo.lineIdx = px;
a_featureInfo.ptIdx = py;
a_featureInfo.flag = 0;
SVzNL3DPoint& a_feature3D = feature3DInfo[px][py];
a_feature3D = a_lineJumpFeature[m].midPt;
}
}
//ˮƽ<CBAE><C6BD>ע
for (int line = 0; line < lineNum_h_raw; line++)
{
std::vector<SSG_featureSemiCircle>& a_lineJumpFeature = lineFeatures_h_raw[line];
for (int m = 0, m_max = (int)a_lineJumpFeature.size(); m < m_max; m++)
{
int py = a_lineJumpFeature[m].lineIdx;
int px = a_lineJumpFeature[m].midPtIdx;
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[px][py];
if (a_featureInfo.featurType == 0)
{
a_featureInfo.clusterID = 0;
a_featureInfo.lineIdx = px;
a_featureInfo.ptIdx = py;
a_featureInfo.flag = 0;
}
a_featureInfo.featurType += 2;
a_featureInfo.featureIdx_h = m;
SVzNL3DPoint& a_feature3DValue = feature3DInfo[px][py];
a_feature3DValue = { a_lineJumpFeature[m].midPt.y, a_lineJumpFeature[m].midPt.x, a_lineJumpFeature[m].midPt.z };
}
}
//<2F><><EFBFBD><EFBFBD>
//<2F><><EFBFBD>õ<EFBFBD><C3B5><EFBFBD>˼<EFBFBD>룬<EFBFBD>ع<EFBFBD>˼·<CBBC><C2B7><EFBFBD>и<EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
std::vector<std::vector< SVzNL2DPoint>> clusters; //ֻ<><D6BB>¼λ<C2BC><CEBB>
std::vector<SVzNL3DRangeD> clustersRoi3D;
int clusterID = 1;
int clusterCheckWin = 5;
for (int y = 0; y < lineNum_h_raw; y++)
{
for (int x = 0; x < lineNum; x++)
{
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y];
if ( (0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>
continue;
SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y];
SVzNL3DRangeD a_clusterRoi;
a_clusterRoi.xRange.min = a_feature3DValue.x;
a_clusterRoi.xRange.max = a_feature3DValue.x;
a_clusterRoi.yRange.min = a_feature3DValue.y;
a_clusterRoi.yRange.max = a_feature3DValue.y;
a_clusterRoi.zRange.min = a_feature3DValue.z;
a_clusterRoi.zRange.max = a_feature3DValue.z;
SVzNL2DPoint a_seedPos = {x, y};
std::vector< SVzNL2DPoint> a_cluster;
a_cluster.push_back(a_seedPos);
wd_pointClustering2D(
featureInfoMask,//int<6E><74><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǻ<EFBFBD>clusterID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>flag
feature3DInfo,//double,<2C><>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
clusterCheckWin, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
growParam,//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
clusterID, //<2F><>ǰCluster<65><72>ID
a_cluster, //result
a_clusterRoi
);
clusters.push_back(a_cluster);
clustersRoi3D.push_back(a_clusterRoi);
clusterID++;
}
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//ȡ<><C8A1>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int clusterSize = (int)clusters.size();
int bestClusterIdx = -1;
double minZ = -1;
double wTh1 = gasFillingPortPara.innerD * 0.8;
double wTh2 = gasFillingPortPara.outerD * 1.1;
for (int i = 0; i < clusterSize; i++)
{
SVzNL3DRangeD& a_roi = clustersRoi3D[i];
double xWidth = a_roi.xRange.max - a_roi.xRange.min;
double yWidth = a_roi.yRange.max - a_roi.yRange.min;
double meanZ = (a_roi.zRange.min + a_roi.zRange.max) / 2;
if ((xWidth > wTh1) && (xWidth < wTh2) && (yWidth > wTh1) && (yWidth < wTh2))
{
if (minZ < 0)
{
minZ = meanZ;
bestClusterIdx = i;
}
else
{
if (minZ > meanZ)
{
minZ = meanZ;
bestClusterIdx = i;
}
}
}
}
if (minZ < 0)
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return resultPose;
}
std::vector< SVzNL2DPoint>& obj_cluster = clusters[bestClusterIdx];
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD>ϵĵ<CFB5>
gasFillingPortPosition ver 1.1.0 改进了加气口顶面点提取的准确度。
2025-12-16 23:09:30 +08:00
//<2F><><EFBFBD>Ƚ<EFBFBD><C8BD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD>ñ<EFBFBD>־
for (int line = 0; line < lineNum; line++)
{
std::vector<SSG_featureSemiCircle>& line_invalidFeature = invalidFeatures_v_raw[line];
for (int n = 0; n < line_invalidFeature.size(); n++)
{
SSG_featureSemiCircle& a_invalidFeature = line_invalidFeature[n];
int lineIdx = a_invalidFeature.lineIdx;
for (int m = a_invalidFeature.startPtIdx; m <= a_invalidFeature.endPtIdx; m++)
{
SVzNL3DPosition& a_pt3d = scanLines[lineIdx][m];
a_pt3d.nPointIdx = 2;
}
}
}
for (int line = 0; line < lineNum_h_raw; line++)
{
std::vector<SSG_featureSemiCircle>& line_invalidFeature = invalidFeatures_h_raw[line];
for (int m = 0, m_max = (int)line_invalidFeature.size(); m < m_max; m++)
{
for (int n = 0; n < line_invalidFeature.size(); n++)
{
SSG_featureSemiCircle& a_invalidFeature = line_invalidFeature[n];
int ptIdx = a_invalidFeature.lineIdx;
for (int m = a_invalidFeature.startPtIdx; m <= a_invalidFeature.endPtIdx; m++)
{
SVzNL3DPosition& a_pt3d = scanLines[m][ptIdx];
a_pt3d.nPointIdx = 3;
}
}
}
}
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
gasFillingPortPosition ver 1.0.0 加气口定位程序初始版本
2025-12-16 21:00:17 +08:00
std::vector< cv::Point3f> planePts;
for (int i = 0, i_max = (int)obj_cluster.size(); i < i_max; i++)
{
SVzNL2DPoint a_pos = obj_cluster[i];
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[a_pos.x][a_pos.y];
int type_v = a_featureInfo.featurType & 0x01;
int type_h = a_featureInfo.featurType & 0x02;
if (type_v > 0)
{
int lineIdx = a_featureInfo.lineIdx;
int featureIdx = a_featureInfo.featureIdx_v;
SSG_featureSemiCircle& a_feature = lineFeatures_v_raw[lineIdx][featureIdx];
for (int m = a_feature.startPtIdx; m <= a_feature.endPtIdx; m++)
{
SVzNL3DPosition& a_pt3d = scanLines[lineIdx][m];
if ((a_pt3d.nPointIdx == 0) && (a_pt3d.pt3D.z > 1e-4))
{
a_pt3d.nPointIdx = 1;
cv::Point3f a_planePt = { (float)a_pt3d.pt3D.x, (float)a_pt3d.pt3D.y, (float)a_pt3d.pt3D.z };
planePts.push_back(a_planePt);
}
}
}
if (type_h > 0)
{
int ptIdx = a_featureInfo.ptIdx;
int featureIdx = a_featureInfo.featureIdx_h;
SSG_featureSemiCircle& a_feature = lineFeatures_h_raw[ptIdx][featureIdx];
for (int m = a_feature.startPtIdx; m <= a_feature.endPtIdx; m++)
{
SVzNL3DPosition& a_pt3d = scanLines[m][ptIdx];
if ( (a_pt3d.nPointIdx == 0) && (a_pt3d.pt3D.z >1e-4))
{
a_pt3d.nPointIdx = 1;
cv::Point3f a_planePt = { (float)a_pt3d.pt3D.x, (float)a_pt3d.pt3D.y, (float)a_pt3d.pt3D.z };
planePts.push_back(a_planePt);
}
}
}
}
// <20><><EFBFBD><EFBFBD>ƽ<EFBFBD>棬<EFBFBD><E6A3AC><EFBFBD>㷨<EFBFBD><E3B7A8>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: z = Ax + By + C
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
std::vector<double> res;
vzCaculateLaserPlane(planePts, res);
//<2F><>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊˮƽƽ<C6BD><C6BD>
SSG_planeCalibPara calibPara = adjustPlaneToXYPlane(
res[0], res[1], res[2] //ƽ<>淨<EFBFBD><E6B7A8><EFBFBD><EFBFBD>
);
//ͶӰ
std::vector< SVzNL3DPoint> projectionPts;
double calibMeanZ = 0;
int planePtSize = (int)planePts.size();
for(int i = 0; i < planePtSize; i ++)
{
SVzNL3DPoint a_calibPt;
a_calibPt.x = (float)(planePts[i].x * calibPara.planeCalib[0] + planePts[i].y * calibPara.planeCalib[1] + planePts[i].z * calibPara.planeCalib[2]);
a_calibPt.y = (float)(planePts[i].x * calibPara.planeCalib[3] + planePts[i].y * calibPara.planeCalib[4] + planePts[i].z * calibPara.planeCalib[5]);
a_calibPt.z = (float)(planePts[i].x * calibPara.planeCalib[6] + planePts[i].y * calibPara.planeCalib[7] + planePts[i].z * calibPara.planeCalib[8]);
calibMeanZ += a_calibPt.z;
projectionPts.push_back(a_calibPt);
}
calibMeanZ = calibMeanZ / planePtSize;
//Բ<><D4B2>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
SVzNL3DPoint calibCenter;
double radius = 0;
double fittingErr = fitCircleByLeastSquare(
projectionPts,
calibCenter,
radius);
calibCenter.z = calibMeanZ;
//center<65><72><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ȥ
SVzNL3DPoint center;
center.x = (float)(calibCenter.x * calibPara.invRMatrix[0] + calibCenter.y * calibPara.invRMatrix[1] + calibCenter.z * calibPara.invRMatrix[2]);
center.y = (float)(calibCenter.x * calibPara.invRMatrix[3] + calibCenter.y * calibPara.invRMatrix[4] + calibCenter.z * calibPara.invRMatrix[5]);
center.z = (float)(calibCenter.x * calibPara.invRMatrix[6] + calibCenter.y * calibPara.invRMatrix[7] + calibCenter.z * calibPara.invRMatrix[8]);
resultPose.x = center.x;
resultPose.y = center.y;
resultPose.z = center.z;
resultPose.x_roll = res[0];
resultPose.y_pitch = res[1];
resultPose.z_yaw = res[2];
return resultPose;
}
Reference in New Issue Copy Permalink