326 lines
10 KiB
C++
326 lines
10 KiB
C++
#include <vector>
|
||
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include "gasFillingPortPosition_Export.h"
|
||
#include <opencv2/opencv.hpp>
|
||
#include <limits>
|
||
|
||
//读版本号
|
||
//version 1.0.0 : base version release to customer
|
||
std::string m_strVersion = "1.0.0";
|
||
const char* wd_gasFillingPortPositionVersion(void)
|
||
{
|
||
return m_strVersion.c_str();
|
||
}
|
||
|
||
//提取加气口中心位姿
|
||
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;
|
||
//提取直线段特征
|
||
//垂直跳变特征提取
|
||
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;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 390)
|
||
int kkk = 1;
|
||
|
||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||
if (linePtNum != (int)lineData.size())
|
||
isGridData = false;
|
||
|
||
//滤波，滤除异常点
|
||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||
|
||
std::vector<SSG_featureSemiCircle> line_features;
|
||
int dataSize = (int)lineData.size();
|
||
wd_simpleLineSegment(
|
||
lineData,
|
||
line,
|
||
lineLenRange,
|
||
lineSegPara,
|
||
line_features);
|
||
lineFeatures_v_raw.push_back(line_features);
|
||
}
|
||
|
||
if (false == isGridData)//数据不是网格格式
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return resultPose;
|
||
}
|
||
|
||
//生成水平扫描
|
||
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; //将原始数据的序列清0（会转义使用）
|
||
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特征提取
|
||
std::vector<std::vector<SSG_featureSemiCircle>> lineFeatures_h_raw;
|
||
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];
|
||
//滤波，滤除异常点
|
||
int ptNum = (int)lineData.size();
|
||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam);
|
||
|
||
std::vector<SSG_featureSemiCircle> line_features;
|
||
int dataSize = (int)lineData.size();
|
||
wd_simpleLineSegment(
|
||
lineData,
|
||
line,
|
||
lineLenRange,
|
||
lineSegPara,
|
||
line_features);
|
||
lineFeatures_h_raw.push_back(line_features);
|
||
}
|
||
|
||
//标注
|
||
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);
|
||
}
|
||
|
||
//垂直标注
|
||
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;
|
||
}
|
||
}
|
||
//水平标注
|
||
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 };
|
||
}
|
||
}
|
||
//聚类
|
||
//采用迭代思想，回归思路进行高效聚类
|
||
std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
|
||
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)) //非特征或已经处理
|
||
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，记录特征标记和clusterID，附加一个flag
|
||
feature3DInfo,//double,记录坐标信息
|
||
clusterCheckWin, //搜索窗口
|
||
growParam,//聚类条件
|
||
clusterID, //当前Cluster的ID
|
||
a_cluster, //result
|
||
a_clusterRoi
|
||
);
|
||
clusters.push_back(a_cluster);
|
||
clustersRoi3D.push_back(a_clusterRoi);
|
||
clusterID++;
|
||
}
|
||
}
|
||
//聚类结果分析
|
||
//取最前面的一个聚类
|
||
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];
|
||
//提取端面上的点
|
||
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);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
// 拟合平面，计算法向
|
||
//计算面参数: z = Ax + By + C
|
||
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
|
||
std::vector<double> res;
|
||
vzCaculateLaserPlane(planePts, res);
|
||
//将平面调整为水平平面
|
||
SSG_planeCalibPara calibPara = adjustPlaneToXYPlane(
|
||
res[0], res[1], res[2] //平面法向量
|
||
);
|
||
|
||
//投影
|
||
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;
|
||
//圆最小二乘拟合
|
||
SVzNL3DPoint calibCenter;
|
||
double radius = 0;
|
||
double fittingErr = fitCircleByLeastSquare(
|
||
projectionPts,
|
||
calibCenter,
|
||
radius);
|
||
calibCenter.z = calibMeanZ;
|
||
|
||
//center点旋转回去
|
||
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;
|
||
|
||
} |