425 lines
11 KiB
C++
425 lines
11 KiB
C++
#include <vector>
|
||
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include "rodAndBarDetection_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_rodAndBarDetectionVersion(void)
|
||
{
|
||
return m_strVersion.c_str();
|
||
}
|
||
|
||
SVzNL3DPoint getArcPeak(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
SWD_segFeature & a_arcFeature,
|
||
SVzNL2DPoint& arcPos)
|
||
{
|
||
SVzNL3DPoint arcPeak = scanLines[a_arcFeature.lineIdx][a_arcFeature.startPtIdx].pt3D;
|
||
for (int i = a_arcFeature.startPtIdx+1; i <= a_arcFeature.endPtIdx; i++)
|
||
{
|
||
if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //跳开空点
|
||
{
|
||
if (arcPeak.z > scanLines[a_arcFeature.lineIdx][i].pt3D.z)
|
||
{
|
||
arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D;
|
||
arcPos = { a_arcFeature.lineIdx , i };
|
||
}
|
||
}
|
||
}
|
||
return arcPeak;
|
||
}
|
||
|
||
SVzNL3DPoint getArcPeak_parabolaFitting(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
SWD_segFeature& a_arcFeature,
|
||
SVzNL2DPoint& arcPos)
|
||
{
|
||
std::vector<cv::Point2d> points;
|
||
for (int i = a_arcFeature.startPtIdx + 1; i <= a_arcFeature.endPtIdx; i++)
|
||
{
|
||
if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //跳开空点
|
||
{
|
||
cv::Point2d a_pt2D;
|
||
if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4)
|
||
{
|
||
a_pt2D.x = scanLines[a_arcFeature.lineIdx][i].pt3D.y;
|
||
a_pt2D.y = scanLines[a_arcFeature.lineIdx][i].pt3D.z;
|
||
points.push_back(a_pt2D);
|
||
}
|
||
}
|
||
}
|
||
double a, b, c, mse, max_err;
|
||
//抛物线最小二乘拟合 y = ax ^ 2 + bx + c
|
||
bool result = leastSquareParabolaFitEigen(
|
||
points,
|
||
a, b, c,
|
||
mse, max_err);
|
||
double yP = -b / (2 * a);
|
||
//寻找与yP最近的点作为Peak点
|
||
SVzNL3DPoint arcPeak;
|
||
double minDist = -1;
|
||
for (int i = a_arcFeature.startPtIdx + 1; i <= a_arcFeature.endPtIdx; i++)
|
||
{
|
||
if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //跳开空点
|
||
{
|
||
double dist = abs(scanLines[a_arcFeature.lineIdx][i].pt3D.y - yP);
|
||
if (minDist < 0)
|
||
{
|
||
minDist = dist;
|
||
arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D;
|
||
arcPos = { a_arcFeature.lineIdx , i };
|
||
}
|
||
else
|
||
{
|
||
if(minDist > dist)
|
||
{
|
||
minDist = dist;
|
||
arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D;
|
||
arcPos = { a_arcFeature.lineIdx , i };
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return arcPeak;
|
||
}
|
||
|
||
//投影，提取ROI内的数据
|
||
void xoyROIProjection(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const double* rtMatrix,
|
||
SSG_ROIRectD& roi_xoy,
|
||
std::vector<SVzNL3DPoint>& projectPoints
|
||
)
|
||
{
|
||
int lineNum = (int)scanLines.size();
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
std::vector<SVzNL3DPosition>& a_line = scanLines[line];
|
||
int ptNum = (int)a_line.size();
|
||
for (int i = 0; i < (int)a_line.size(); i++)
|
||
{
|
||
SVzNL3DPoint a_pt = a_line[i].pt3D;
|
||
if (a_pt.z < 1e-4)
|
||
continue;
|
||
double x = a_pt.x * rtMatrix[0] + a_pt.y * rtMatrix[1] + a_pt.z * rtMatrix[2];
|
||
double y = a_pt.x * rtMatrix[3] + a_pt.y * rtMatrix[4] + a_pt.z * rtMatrix[5];
|
||
double z = a_pt.x * rtMatrix[6] + a_pt.y * rtMatrix[7] + a_pt.z * rtMatrix[8];
|
||
if ((x >= roi_xoy.left) && (x <= roi_xoy.right) &&
|
||
(y >= roi_xoy.top) && (y <= roi_xoy.bottom))
|
||
{
|
||
a_pt.x = x;
|
||
a_pt.y = y;
|
||
a_pt.z = z;
|
||
projectPoints.push_back(a_pt);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
SVzNLRangeD getZRange(std::vector<SVzNL3DPoint>& projectPoints)
|
||
{
|
||
int ptNum = (int)projectPoints.size();
|
||
SVzNLRangeD zRange;
|
||
zRange.min = DBL_MAX;
|
||
zRange.max = DBL_MIN;
|
||
for (int i = 0; i < ptNum; i++)
|
||
{
|
||
zRange.min = zRange.min > projectPoints[i].z ? projectPoints[i].z : zRange.min;
|
||
zRange.max = zRange.max < projectPoints[i].z ? projectPoints[i].z : zRange.max;
|
||
}
|
||
return zRange;
|
||
}
|
||
|
||
void zCutPointClouds(
|
||
std::vector<SVzNL3DPoint>& projectPoints,
|
||
SVzNLRangeD& zRange,
|
||
std::vector<SVzNL3DPoint>& cutLayerPoints)
|
||
{
|
||
int ptNum = (int)projectPoints.size();
|
||
for (int i = 0; i < ptNum; i++)
|
||
{
|
||
if ((projectPoints[i].z >= zRange.min) && (projectPoints[i].z <= zRange.max))
|
||
cutLayerPoints.push_back(projectPoints[i]);
|
||
}
|
||
}
|
||
|
||
SVzNL3DPoint getXoYCentroid(std::vector<SVzNL3DPoint>& points)
|
||
{
|
||
int ptNum = (int)points.size();
|
||
SVzNL3DPoint centroid = { 0.0, 0.0, 0.0 };
|
||
if (ptNum == 0)
|
||
return centroid;
|
||
for (int i = 0; i < ptNum; i++)
|
||
{
|
||
centroid.x += points[i].x;
|
||
centroid.y += points[i].y;
|
||
}
|
||
centroid.x = centroid.x / ptNum;
|
||
centroid.y = centroid.y / ptNum;
|
||
return centroid;
|
||
}
|
||
|
||
SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
|
||
{
|
||
SVzNL3DPoint _r_pt;
|
||
_r_pt.x = pt3D.x * matrix3d[0] + pt3D.y * matrix3d[1] + pt3D.z * matrix3d[2];
|
||
_r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5];
|
||
_r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8];
|
||
return _r_pt;
|
||
}
|
||
|
||
void sx_hexHeadScrewMeasure(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
bool isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道
|
||
const SSG_cornerParam cornerPara,
|
||
const SSG_outlierFilterParam filterParam,
|
||
const SSG_treeGrowParam growParam,
|
||
double rodDiameter,
|
||
std::vector<SSX_hexHeadScrewInfo>& screwInfo,
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
int lineNum = (int)scanLines.size();
|
||
if (lineNum == 0)
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_NULL;
|
||
return;
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
std::vector< std::vector<SVzNL3DPosition>> data_lines;
|
||
if (false == isHorizonScan)
|
||
{
|
||
data_lines.resize(lineNum);
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
data_lines[line].insert(data_lines[line].end(), scanLines[line].begin(), scanLines[line].end());
|
||
for (int j = 0, j_max = (int)data_lines[line].size(); j < j_max; j++)
|
||
{
|
||
data_lines[line][j].nPointIdx = j;
|
||
scanLines[line][j].nPointIdx = 0; //转义复用
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
data_lines.resize(linePtNum);
|
||
for (int i = 0; i < linePtNum; i++)
|
||
data_lines[i].resize(lineNum);
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
data_lines[j][line] = scanLines[line][j];
|
||
data_lines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
||
data_lines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
||
}
|
||
|
||
}
|
||
|
||
lineNum = linePtNum;
|
||
linePtNum = (int)data_lines[0].size();
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0, j_max = (int)data_lines[line].size(); j < j_max; j++)
|
||
data_lines[line][j].nPointIdx = j;
|
||
}
|
||
}
|
||
|
||
std::vector<std::vector<SWD_segFeature>> arcFeatures;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 329)
|
||
int kkk = 1;
|
||
std::vector<SVzNL3DPosition>& lineData = data_lines[line];
|
||
|
||
//滤波，滤除异常点
|
||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||
std::vector<SWD_segFeature> line_ringArcs;
|
||
int dataSize = (int)lineData.size();
|
||
SVzNLRangeD arcWidth;
|
||
arcWidth.min = rodDiameter / 2;
|
||
arcWidth.max = rodDiameter * 1.5;
|
||
//提取Arc特征
|
||
wd_getRingArcFeature(
|
||
lineData,
|
||
line, //当前扫描线序号
|
||
cornerPara,
|
||
arcWidth, //环宽度，以半径为基准，对应60度角
|
||
line_ringArcs //环
|
||
);
|
||
arcFeatures.push_back(line_ringArcs);
|
||
}
|
||
//特征生长
|
||
std::vector<SWD_segFeatureTree> growTrees;
|
||
wd_getSegFeatureGrowingTrees(
|
||
arcFeatures,
|
||
growTrees,
|
||
growParam);
|
||
|
||
if (growTrees.size() == 0)
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return;
|
||
}
|
||
int objNum = (int)growTrees.size();
|
||
|
||
//置标志，用于debug
|
||
for (int i = 0; i < objNum; i++)
|
||
{
|
||
int nodeNum = (int)growTrees[i].treeNodes.size();
|
||
for (int j = 0; j < nodeNum; j++)
|
||
{
|
||
int lineIdx, ptIdx;
|
||
if (false == isHorizonScan)
|
||
{
|
||
lineIdx = growTrees[i].treeNodes[j].lineIdx;
|
||
for (int m = growTrees[i].treeNodes[j].startPtIdx; m <= growTrees[i].treeNodes[j].endPtIdx; m++)
|
||
{
|
||
ptIdx = m;
|
||
scanLines[lineIdx][ptIdx].nPointIdx = 1;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
ptIdx = growTrees[i].treeNodes[j].lineIdx;
|
||
for (int m = growTrees[i].treeNodes[j].startPtIdx; m <= growTrees[i].treeNodes[j].endPtIdx; m++)
|
||
{
|
||
lineIdx = m;
|
||
scanLines[lineIdx][ptIdx].nPointIdx = 1;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
//逐个目标处理
|
||
for (int i = 0; i < objNum; i++)
|
||
{
|
||
//空间直线拟合
|
||
std::vector<SVzNL3DPoint> fitPoints;
|
||
std::vector<SVzNL2DPoint> fit2DPos;
|
||
int nodeSize = (int)growTrees[i].treeNodes.size();
|
||
for (int j = 0; j < nodeSize; j++)
|
||
{
|
||
SVzNL2DPoint arcPos;
|
||
SVzNL3DPoint a_pt = getArcPeak_parabolaFitting(data_lines, growTrees[i].treeNodes[j], arcPos);
|
||
//SVzNL3DPoint a_pt = getArcPeak(data_lines, growTrees[i].treeNodes[j], arcPos);
|
||
fitPoints.push_back(a_pt);
|
||
fit2DPos.push_back(arcPos);
|
||
}
|
||
if (fitPoints.size() < 27)
|
||
continue;
|
||
//去除头尾各5个点，防止在端部和根部扫描时的数据有干扰
|
||
fitPoints.erase(fitPoints.begin(), fitPoints.begin() + 10);
|
||
fit2DPos.erase(fit2DPos.begin(), fit2DPos.begin() + 10);
|
||
fitPoints.erase(fitPoints.end() - 5, fitPoints.end());
|
||
fit2DPos.erase(fit2DPos.end() - 5, fit2DPos.end());
|
||
//置标志
|
||
for (int j = 0; j < (int)fit2DPos.size(); j++)
|
||
{
|
||
int lineIdx, ptIdx;
|
||
if (false == isHorizonScan)
|
||
{
|
||
lineIdx = fit2DPos[j].x;
|
||
ptIdx = fit2DPos[j].y;
|
||
}
|
||
else
|
||
{
|
||
lineIdx = fit2DPos[j].y;
|
||
ptIdx = fit2DPos[j].x;
|
||
}
|
||
scanLines[lineIdx][ptIdx].nPointIdx = 2;
|
||
}
|
||
|
||
//拟合
|
||
SVzNL3DPoint P0_center, P1_dir;
|
||
bool result = fitLine3DLeastSquares(fitPoints, P0_center, P1_dir);
|
||
if (false == result)
|
||
continue;
|
||
//投影
|
||
//计算旋转向量
|
||
SVzNL3DPoint vector1 = P1_dir;
|
||
SVzNL3DPoint vector2 = { 0, 0, -1.0 };
|
||
SSG_planeCalibPara rotatePara = wd_computeRTMatrix( vector1, vector2);
|
||
//
|
||
SVzNL3DPoint P0_rotate = _ptRotate(P0_center, rotatePara.planeCalib);
|
||
SSG_ROIRectD roi_xoy;
|
||
roi_xoy.left = P0_rotate.x - rodDiameter * 2; //2D范围
|
||
roi_xoy.right = P0_rotate.x + rodDiameter * 2; //2D范围
|
||
roi_xoy.top = P0_rotate.y - rodDiameter * 2; //2D范围
|
||
roi_xoy.bottom = P0_rotate.y + rodDiameter * 2; //2D范围
|
||
|
||
#if 1
|
||
std::vector< SVzNL3DPoint> verifyData;
|
||
for (int m = 0; m < (int)fitPoints.size(); m++)
|
||
{
|
||
SVzNL3DPoint rPt = _ptRotate(fitPoints[m], rotatePara.planeCalib);
|
||
verifyData.push_back(rPt);
|
||
}
|
||
#endif
|
||
std::vector< SVzNL3DPoint> roiProjectionData;
|
||
xoyROIProjection(data_lines, rotatePara.planeCalib, roi_xoy, roiProjectionData);
|
||
//取端面
|
||
SVzNLRangeD zRange = getZRange(roiProjectionData);
|
||
SVzNLRangeD cutZRange;
|
||
cutZRange.min = zRange.min;
|
||
cutZRange.max = zRange.min + 5.0; //5mm的端面
|
||
std::vector<SVzNL3DPoint> surfacePoints;
|
||
zCutPointClouds(roiProjectionData, cutZRange, surfacePoints);
|
||
//计算中心点
|
||
SVzNL3DPoint projectionCenter;// = getXoYCentroid(surfacePoints);
|
||
SVzNL3DRangeD roi3D = wd_getPointCloudROI(surfacePoints);
|
||
projectionCenter.x = (roi3D.xRange.min + roi3D.xRange.max) / 2;
|
||
projectionCenter.y = (roi3D.yRange.min + roi3D.yRange.max) / 2;
|
||
projectionCenter.z = zRange.min;
|
||
//旋转回原坐标系
|
||
SVzNL3DPoint surfaceCenter = _ptRotate(projectionCenter, rotatePara.invRMatrix);
|
||
//生成Rod信息
|
||
SSX_hexHeadScrewInfo a_rod;
|
||
a_rod.center = surfaceCenter;
|
||
a_rod.axialDir = P1_dir;
|
||
a_rod.rotateAngle = 0;
|
||
screwInfo.push_back(a_rod);
|
||
}
|
||
if (true == isHorizonScan)
|
||
{
|
||
int objNum = (int)screwInfo.size();
|
||
for (int i = 0; i < objNum; i++)
|
||
{
|
||
double tmp = screwInfo[i].center.x;
|
||
screwInfo[i].center.x = screwInfo[i].center.y;
|
||
screwInfo[i].center.y = tmp;
|
||
tmp = screwInfo[i].axialDir.x;
|
||
screwInfo[i].axialDir.x = screwInfo[i].axialDir.y;
|
||
screwInfo[i].axialDir.y = tmp;
|
||
//screwInfo[i].rotateAngle += 90;
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
|
||
|
||
|