924 lines
29 KiB
C++
924 lines
29 KiB
C++
#include <vector>
|
||
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include "workpieceHolePositioning_Export.h"
|
||
#include <opencv2/opencv.hpp>
|
||
#include <limits>
|
||
|
||
//version 1.0.0 : base version release to customer
|
||
//version 1.0.2 : 添加了工件姿态（欧拉角输出)
|
||
//version 1.1.0 : c对工件姿态规范化为中心点（操作点）加三个方向矢量
|
||
//version 1.2.0 : 算法完成了6轴验证
|
||
//version 1.3.0 : (1)算法进行了迭代 (2)对结果进行了分层和排序，输出最上层目标
|
||
std::string m_strVersion = "1.3.0";
|
||
const char* wd_workpieceHolePositioningVersion(void)
|
||
{
|
||
return m_strVersion.c_str();
|
||
}
|
||
|
||
//相机水平安装计算地面调平参数。
|
||
//相机Z轴基本平行地面时，需要以地面为参照，将相机调水平
|
||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||
SSG_planeCalibPara wd_getGroundCalibPara(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||
{
|
||
return sg_getPlaneCalibPara2(scanLines);
|
||
}
|
||
|
||
//相机水平时姿态调平，并去除地面
|
||
void wd_lineDataR(
|
||
std::vector< SVzNL3DPosition>& a_line,
|
||
const double* camPoseR,
|
||
double groundH)
|
||
{
|
||
lineDataRT_vector(a_line, camPoseR, groundH);
|
||
}
|
||
|
||
SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, const 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;
|
||
}
|
||
|
||
//搜索最接近distance的目标
|
||
int distanceSearchObject(SVzNL3DPoint seed, std::vector<SWD_HoleInfo>& holes, double distance, double distDeviation)
|
||
{
|
||
int result = -1;
|
||
int holeSize = (int)holes.size();
|
||
double minDistDiff = DBL_MAX;
|
||
int minDistIndex = -1;
|
||
for (int i = 0; i < holeSize; i++)
|
||
{
|
||
if (holes[i].radius < 0)
|
||
continue;
|
||
|
||
double dist = sqrt(pow(seed.x - holes[i].center.x, 2) + pow(seed.y - holes[i].center.y, 2));
|
||
double distDiff = abs(dist - distance);
|
||
if (minDistDiff > distDiff)
|
||
{
|
||
minDistDiff = distDiff;
|
||
minDistIndex = i;
|
||
}
|
||
}
|
||
if ((minDistIndex >= 0) && (minDistDiff < distDeviation))
|
||
result = minDistIndex;
|
||
return result;
|
||
}
|
||
|
||
//搜索最接近distance且角度为angle的目标, 以角度为优先
|
||
int angleConditionDistanceSearch(
|
||
SVzNL3DPoint seed, SVzNL3DPoint angleSide,
|
||
std::vector<SWD_HoleInfo>& holes,
|
||
double distance, double distDeviation,
|
||
SVzNLRangeD angleRange)
|
||
{
|
||
int result = -1;
|
||
int holeSize = (int)holes.size();
|
||
std::vector< int> distValidHoleIndex;
|
||
for (int i = 0; i < holeSize; i++)
|
||
{
|
||
if (holes[i].radius < 0)
|
||
continue;
|
||
|
||
double dist = sqrt(pow(seed.x - holes[i].center.x, 2) + pow(seed.y - holes[i].center.y, 2));
|
||
double distDiff = abs(dist - distance);
|
||
if (distDiff < distDeviation)
|
||
{
|
||
distValidHoleIndex.push_back(i);
|
||
}
|
||
}
|
||
if (distValidHoleIndex.size() == 1)
|
||
{
|
||
int idx = distValidHoleIndex[0];
|
||
double angle = computeXOYVertexAngle(seed, angleSide, holes[idx].center);
|
||
if( (angle >= angleRange.min) &&(angle <= angleRange.max))
|
||
result = idx;
|
||
}
|
||
else if (distValidHoleIndex.size() > 1)
|
||
{
|
||
double bestAngle = (angleRange.min + angleRange.max) / 2;
|
||
double minAngleDeviateion = DBL_MAX;
|
||
int minAngleIdx = -1;
|
||
for (int i = 0, i_max = (int)distValidHoleIndex.size(); i < i_max; i++)
|
||
{
|
||
int idx = distValidHoleIndex[i];
|
||
double angle = computeXOYVertexAngle(seed, angleSide, holes[idx].center);
|
||
if ((angle >= angleRange.min) && (angle <= angleRange.max))
|
||
{
|
||
double angleDiff = abs(angle - bestAngle);
|
||
if (minAngleDeviateion > angleDiff)
|
||
{
|
||
minAngleDeviateion = angleDiff;
|
||
minAngleIdx = idx;
|
||
}
|
||
}
|
||
}
|
||
result = minAngleIdx;
|
||
}
|
||
return result;
|
||
}
|
||
|
||
double _getMeanZ(std::vector<std::vector<double>>& quantiValue, SVzNL3DPoint seed, SVzNLRect& roi2D, double rectR)
|
||
{
|
||
int cols = (int)quantiValue.size();
|
||
int rows = (int)quantiValue[0].size();
|
||
|
||
int px = (int)seed.x - roi2D.left;
|
||
int py = (int)seed.y - roi2D.top;
|
||
int win = (int)rectR;
|
||
int hist = 0;
|
||
double zSum = 0;
|
||
for (int i = -win; i <= win; i++)
|
||
{
|
||
for (int j = -win; j <= win; j++)
|
||
{
|
||
int qx = px + i;
|
||
int qy = py + j;
|
||
if ((qx >= 0) && (qx < cols) && (qy >= 0) && (qy < rows))
|
||
{
|
||
if (quantiValue[qx][qy] > 1e-4)
|
||
{
|
||
zSum += quantiValue[qx][qy];
|
||
hist++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
if (hist == 0)
|
||
return 0;
|
||
else
|
||
return (zSum / hist);
|
||
}
|
||
|
||
void _updateROI(SSG_ROIRectD& roi, SVzNL3DPoint& a_pt)
|
||
{
|
||
if (a_pt.z > 1E-4)
|
||
{
|
||
if (roi.left < 0)
|
||
{
|
||
roi.left = a_pt.x;
|
||
roi.right = a_pt.x;
|
||
roi.left = a_pt.y;
|
||
roi.right = a_pt.y;
|
||
}
|
||
else
|
||
{
|
||
if (roi.left > a_pt.x)
|
||
roi.left = a_pt.x;
|
||
if (roi.right < a_pt.x)
|
||
roi.right = a_pt.x;
|
||
if (roi.top > a_pt.y)
|
||
roi.top = a_pt.y;
|
||
if (roi.bottom < a_pt.y)
|
||
roi.bottom = a_pt.y;
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
void _updateRenge(SVzNLRange& range, int idx)
|
||
{
|
||
if (range.nMin < 0)
|
||
{
|
||
range.nMin = idx;
|
||
range.nMax = idx;
|
||
}
|
||
else
|
||
{
|
||
if (range.nMin > idx)
|
||
range.nMin = idx;
|
||
if (range.nMax < idx)
|
||
range.nMax = idx;
|
||
}
|
||
return;
|
||
}
|
||
|
||
bool _checkExist(int id, std::vector<int>& buff)
|
||
{
|
||
for (int i = 0; i < (int)buff.size(); i++)
|
||
{
|
||
if (id == buff[i])
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
void _searchNeighbours(
|
||
int selfId, int chkExtening,
|
||
int sLineIdx, int eLineIdx,
|
||
SVzNLRange ptIdxRange,
|
||
std::vector<std::vector<int>>& treeMask,
|
||
std::vector<int>& neighbours)
|
||
{
|
||
int lineNum = (int)treeMask.size();
|
||
int ptNum = treeMask[0].size();
|
||
for (int line = sLineIdx - chkExtening; line <= eLineIdx+ chkExtening; line++)
|
||
{
|
||
if ((line >= 0) && (line < lineNum))
|
||
{
|
||
for (int ptIdx = ptIdxRange.nMin - chkExtening; ptIdx <= ptIdxRange.nMax + chkExtening; ptIdx++)
|
||
{
|
||
if ((ptIdx >= 0) && (ptIdx < ptNum))
|
||
{
|
||
if ((treeMask[line][ptIdx] >= 0) && (treeMask[line][ptIdx] != selfId))
|
||
{
|
||
bool isExist = _checkExist(treeMask[line][ptIdx], neighbours);
|
||
if (false == isExist)
|
||
neighbours.push_back(treeMask[line][ptIdx]);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
bool _compareByXValue(WD_workpieceInfo& a, WD_workpieceInfo& b)
|
||
{
|
||
return a.center.x < b.center.x;
|
||
}
|
||
void _getYTopLine(
|
||
std::vector< WD_workpieceInfo>& workpieceSrc,
|
||
std::vector< WD_workpieceInfo>& firstLine,
|
||
std::vector< WD_workpieceInfo>& restWorkpiece,
|
||
double yLayerTh)
|
||
{
|
||
//搜索Y最小值
|
||
if (workpieceSrc.size() == 0)
|
||
return;
|
||
|
||
double minY = workpieceSrc[0].center.y;
|
||
for (int i = 1; i < (int)workpieceSrc.size(); i++)
|
||
{
|
||
if (minY > workpieceSrc[i].center.y)
|
||
minY = workpieceSrc[i].center.y;
|
||
}
|
||
double topLayerTh = minY + yLayerTh;
|
||
for (int i = 0; i < (int)workpieceSrc.size(); i++)
|
||
{
|
||
if (workpieceSrc[i].center.y < topLayerTh)
|
||
firstLine.push_back(workpieceSrc[i]);
|
||
else
|
||
restWorkpiece.push_back(workpieceSrc[i]);
|
||
}
|
||
std::sort(firstLine.begin(), firstLine.end(), _compareByXValue);
|
||
return;
|
||
}
|
||
//工件孔定位
|
||
void wd_workpieceHolePositioning(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
|
||
const WD_workpieceHoleParam workpiecePara,
|
||
const SSG_lineSegParam lineSegPara,
|
||
const SSG_outlierFilterParam filterParam,
|
||
const SSG_treeGrowParam growParam,
|
||
const SSG_planeCalibPara groundCalibPara,
|
||
std::vector< WD_workpieceInfo>& workpiecePositioning,
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
|
||
int lineNum = (int)scanLinesInput.size();
|
||
std::vector< std::vector<SVzNL3DPosition>> scanLines;
|
||
scanLines.resize(lineNum);
|
||
int linePtNum = (int)scanLinesInput[0].size();
|
||
bool isGridData = true;
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
if (linePtNum != (int)scanLinesInput[i].size())
|
||
isGridData = false;
|
||
|
||
scanLines[i].resize(scanLinesInput[i].size());
|
||
std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 使用std::copy算法
|
||
|
||
for (int j = 0; j < (int)scanLinesInput[i].size(); j++)
|
||
scanLinesInput[i][j].nPointIdx = 0; //清零，用于debug时记录信息
|
||
}
|
||
if (false == isGridData)//数据不是网格格式
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return;
|
||
}
|
||
for (int i = 0; i < lineNum; i++)
|
||
{ //行处理
|
||
//调平，去除地面
|
||
wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
|
||
}
|
||
|
||
//生成量化数据，以1mm为量化尺度，用于确定工件表面高度
|
||
SVzNL3DRangeD roi3D = sg_getScanDataROI_vector( scanLines);
|
||
SVzNLRect roi2D;
|
||
roi2D.left = (int)roi3D.xRange.min;
|
||
roi2D.right = (int)roi3D.xRange.max;
|
||
roi2D.top = (int)roi3D.yRange.min;
|
||
roi2D.bottom = (int)roi3D.yRange.max;
|
||
int quanti_X = roi2D.right - roi2D.left + 1;
|
||
int quanti_Y = roi2D.bottom - roi2D.top + 1;
|
||
std::vector<std::vector<double>> quantiValue;
|
||
std::vector<std::vector<int>> quantiHist;
|
||
quantiValue.resize(quanti_X);
|
||
quantiHist.resize(quanti_X);
|
||
for (int i = 0; i < quanti_X; i++)
|
||
{
|
||
quantiValue[i].resize(quanti_Y);
|
||
std::fill(quantiValue[i].begin(), quantiValue[i].end(), 0);//初始化为0
|
||
quantiHist[i].resize(quanti_Y);
|
||
std::fill(quantiHist[i].begin(), quantiHist[i].end(), 0);//初始化为0
|
||
}
|
||
//以1mm尺度量化
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
SVzNL3DPoint& a_pt = scanLines[line][j].pt3D;
|
||
if (a_pt.z > 1e-4)
|
||
{
|
||
int qx = (int)a_pt.x - roi2D.left;
|
||
int qy = (int)a_pt.y - roi2D.top;
|
||
quantiValue[qx][qy] += a_pt.z;
|
||
quantiHist[qx][qy] += 1;
|
||
}
|
||
}
|
||
}
|
||
for (int ix = 0; ix < quanti_X; ix++)
|
||
{
|
||
for (int iy = 0; iy < quanti_Y; iy++)
|
||
{
|
||
if (quantiHist[ix][iy] > 0)
|
||
quantiValue[ix][iy] = quantiValue[ix][iy] / quantiHist[ix][iy];
|
||
}
|
||
}
|
||
|
||
std::vector<std::vector<int>> pointMask;
|
||
pointMask.resize(lineNum);
|
||
|
||
//提取空白线段特征（孔特征）
|
||
std::vector<std::vector<SWD_segFeature>> holeGaps;
|
||
//提取线段端点特征
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 1047)
|
||
int kkk = 1;
|
||
|
||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||
pointMask[line].resize(lineData.size());
|
||
std::fill(pointMask[line].begin(), pointMask[line].end(), 0);//初始化为0
|
||
//滤波，滤除异常点
|
||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||
|
||
std::vector<SSG_RUN> segs;
|
||
wd_getLineDataNullIntervals(lineData, lineSegPara, segs);
|
||
|
||
//将seg端点作为边缘点。做了地面调平后，垂直孔的内侧在XY平面上均为边缘点。
|
||
std::vector<SWD_segFeature> line_gaps;
|
||
for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
|
||
{
|
||
int ptIdx_1 = segs[i].start;
|
||
int ptIdx_2 = segs[i].start + segs[i].len - 1;
|
||
SWD_segFeature a_gap;
|
||
a_gap.lineIdx = line;
|
||
a_gap.startPtIdx = ptIdx_1;
|
||
a_gap.endPtIdx = ptIdx_2;
|
||
a_gap.startPt = lineData[ptIdx_1].pt3D;
|
||
a_gap.endPt = lineData[ptIdx_2].pt3D;
|
||
a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y);
|
||
line_gaps.push_back(a_gap);
|
||
}
|
||
holeGaps.push_back(line_gaps);
|
||
}
|
||
//特征生长
|
||
std::vector<SWD_segFeatureTree> segTrees_v;
|
||
wd_getSegFeatureGrowingTrees_2(holeGaps, segTrees_v, growParam);
|
||
|
||
//生成水平扫描
|
||
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<SWD_segFeature>> holeGaps_h;
|
||
for (int line = 0; line < linePtNum; 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_RUN> segs;
|
||
wd_getLineDataNullIntervals(lineData, lineSegPara, segs);
|
||
|
||
//将seg端点作为边缘点。做了地面调平后，垂直孔的内侧在XY平面上均为边缘点。
|
||
std::vector<SWD_segFeature> line_gaps;
|
||
for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
|
||
{
|
||
int ptIdx_1 = segs[i].start;
|
||
int ptIdx_2 = segs[i].start + segs[i].len - 1;
|
||
SWD_segFeature a_gap;
|
||
a_gap.lineIdx = line;
|
||
a_gap.startPtIdx = ptIdx_1;
|
||
a_gap.endPtIdx = ptIdx_2;
|
||
a_gap.startPt = lineData[ptIdx_1].pt3D;
|
||
a_gap.endPt = lineData[ptIdx_2].pt3D;
|
||
a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y);
|
||
line_gaps.push_back(a_gap);
|
||
}
|
||
holeGaps_h.push_back(line_gaps);
|
||
}
|
||
//特征生长
|
||
std::vector<SWD_segFeatureTree> segTrees_h;
|
||
wd_getSegFeatureGrowingTrees_2(holeGaps_h, segTrees_h, growParam);
|
||
|
||
//创建Tree所在孔洞的Mask
|
||
std::vector<std::vector<int>> treeMask_v;
|
||
treeMask_v.resize(lineNum);
|
||
std::vector<std::vector<int>> treeMask_h;
|
||
treeMask_h.resize(lineNum);
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
treeMask_v[i].resize(linePtNum);
|
||
std::fill(treeMask_v[i].begin(), treeMask_v[i].end(), -1);
|
||
treeMask_h[i].resize(linePtNum);
|
||
std::fill(treeMask_h[i].begin(), treeMask_h[i].end(), -1);
|
||
}
|
||
//标注
|
||
std::vector<SSG_treeInfo> treeInfo_v;
|
||
treeInfo_v.resize(segTrees_v.size());
|
||
for (int i = 0; i < (int)segTrees_v.size(); i++)
|
||
{
|
||
SWD_segFeatureTree& a_tree = segTrees_v[i];
|
||
treeInfo_v[i].treeIdx = i;
|
||
treeInfo_v[i].sLineIdx = a_tree.sLineIdx;
|
||
treeInfo_v[i].eLineIdx = a_tree.eLineIdx;
|
||
treeInfo_v[i].vTreeFlag = 0;
|
||
treeInfo_v[i].treeType = 0;
|
||
treeInfo_v[i].roi = {-1, -1, -1, -1};
|
||
int nullPtSize = 0;
|
||
SSG_ROIRectD roi = {-1, -1, -1, -1};
|
||
SVzNLRange ptIdxRange = { -1, -1 };
|
||
if (a_tree.treeNodes.size() > 2)
|
||
{
|
||
for (int m = 0; m < (int)a_tree.treeNodes.size(); m++)
|
||
{
|
||
SWD_segFeature& a_seg = a_tree.treeNodes[m];
|
||
for (int n = a_seg.startPtIdx; n <= a_seg.endPtIdx; n++)
|
||
treeMask_v[a_seg.lineIdx][n] = i;
|
||
|
||
nullPtSize += a_seg.endPtIdx - a_seg.startPtIdx + 1;
|
||
_updateROI(roi, scanLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D);
|
||
_updateROI(roi, scanLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D);
|
||
_updateRenge(ptIdxRange, a_seg.startPtIdx);
|
||
_updateRenge(ptIdxRange, a_seg.endPtIdx);
|
||
//scanLinesInput[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = 0x01;
|
||
//scanLinesInput[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = 0x01;
|
||
}
|
||
treeInfo_v[i].treeType = nullPtSize;
|
||
treeInfo_v[i].roi = roi;
|
||
treeInfo_v[i].ptIdxRange = ptIdxRange;
|
||
}
|
||
}
|
||
std::vector<SSG_treeInfo> treeInfo_h;
|
||
treeInfo_h.resize(segTrees_h.size());
|
||
for (int i = 0; i < (int)segTrees_h.size(); i++)
|
||
{
|
||
SWD_segFeatureTree& a_tree = segTrees_h[i];
|
||
treeInfo_h[i].treeIdx = i;
|
||
treeInfo_h[i].sLineIdx = a_tree.sLineIdx;
|
||
treeInfo_h[i].eLineIdx = a_tree.eLineIdx;
|
||
treeInfo_h[i].vTreeFlag = 0;
|
||
treeInfo_h[i].treeType = 0;
|
||
treeInfo_h[i].roi = { -1, -1, -1, -1 };
|
||
int nullPtSize = 0;
|
||
SSG_ROIRectD roi = { -1, -1, -1, -1 };
|
||
SVzNLRange ptIdxRange = { -1, -1 };
|
||
if (a_tree.treeNodes.size() > 2)
|
||
{
|
||
for (int m = 0; m < (int)a_tree.treeNodes.size(); m++)
|
||
{
|
||
SWD_segFeature& a_seg = a_tree.treeNodes[m];
|
||
for (int n = a_seg.startPtIdx; n <= a_seg.endPtIdx; n++)
|
||
treeMask_h[n][a_seg.lineIdx] = i;
|
||
|
||
nullPtSize += a_seg.endPtIdx - a_seg.startPtIdx + 1;
|
||
_updateROI(roi, scanLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D);
|
||
_updateROI(roi, scanLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D);
|
||
_updateRenge(ptIdxRange, a_seg.startPtIdx);
|
||
_updateRenge(ptIdxRange, a_seg.endPtIdx);
|
||
//scanLinesInput[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx |= 0x02;
|
||
//scanLinesInput[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx |= 0x02;
|
||
}
|
||
treeInfo_h[i].treeType = nullPtSize;
|
||
treeInfo_h[i].roi = roi;
|
||
treeInfo_h[i].ptIdxRange = ptIdxRange;
|
||
}
|
||
}
|
||
//水平和垂直目标合并
|
||
int vTreeSize = (int)segTrees_v.size();
|
||
int hTreeSize = (int)segTrees_h.size();
|
||
std::vector<std::vector<int>> treeHVInfo; //统计垂直和水平的tree的位置信息
|
||
treeHVInfo.resize(vTreeSize);
|
||
for (int i = 0; i < vTreeSize; i++)
|
||
{
|
||
treeHVInfo[i].resize(hTreeSize);
|
||
std::fill(treeHVInfo[i].begin(), treeHVInfo[i].end(), 0);
|
||
}
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
|
||
{
|
||
int idx_v = treeMask_v[line][ptIdx];
|
||
int idx_h = treeMask_h[line][ptIdx];
|
||
if ((idx_v >= 0) && (idx_h >= 0))
|
||
treeHVInfo[idx_v][idx_h]++;
|
||
}
|
||
}
|
||
|
||
//生成候选目标
|
||
std::vector<SSG_intPair> objects;
|
||
for (int i = 0; i < vTreeSize; i++)
|
||
{
|
||
SWD_segFeatureTree& a_tree = segTrees_v[i];
|
||
if ((a_tree.sLineIdx <= 1047) && (a_tree.eLineIdx >= 1047))
|
||
int kkk = 1;
|
||
|
||
int totalSize_v = treeInfo_v[i].treeType;
|
||
int commonSize = 0;
|
||
int hTreeIdx = -1;
|
||
for (int j = 0; j < hTreeSize; j++)
|
||
{
|
||
if (commonSize < treeHVInfo[i][j])
|
||
{
|
||
hTreeIdx = j;
|
||
commonSize = treeHVInfo[i][j];
|
||
}
|
||
}
|
||
if (hTreeIdx >= 0)
|
||
{
|
||
int totalSize_h = treeInfo_h[hTreeIdx].treeType;
|
||
if ((commonSize > totalSize_h / 4) && (commonSize > totalSize_v / 4))
|
||
{
|
||
SSG_intPair a_obj;
|
||
a_obj.data_0 = i;
|
||
a_obj.data_1 = hTreeIdx;
|
||
a_obj.idx = commonSize;
|
||
objects.push_back(a_obj);
|
||
treeInfo_v[i].data = commonSize;
|
||
treeInfo_h[hTreeIdx].data = commonSize;
|
||
}
|
||
}
|
||
}
|
||
|
||
//滤除相邻。每个目标保留一个
|
||
for (int i = 0; i < (int)objects.size(); i++)
|
||
{
|
||
int vTreeIdx = objects[i].data_0;
|
||
if (treeInfo_v[vTreeIdx].vTreeFlag < 0)
|
||
continue;
|
||
|
||
std::vector<int> neighbours;
|
||
_searchNeighbours(vTreeIdx, 3,
|
||
treeInfo_v[vTreeIdx].sLineIdx, treeInfo_v[vTreeIdx].eLineIdx,
|
||
treeInfo_v[vTreeIdx].ptIdxRange, treeMask_v, neighbours);
|
||
|
||
int bestIdx = vTreeIdx;
|
||
int maxValue = treeInfo_v[vTreeIdx].data;
|
||
if (neighbours.size() > 0)
|
||
{
|
||
for (int j = 0; j < (int)neighbours.size(); j++)
|
||
{
|
||
int idx = neighbours[j];
|
||
if (maxValue < treeInfo_v[idx].data)
|
||
{
|
||
maxValue = treeInfo_v[idx].data;
|
||
bestIdx = idx;
|
||
}
|
||
}
|
||
if (bestIdx != vTreeIdx)
|
||
treeInfo_v[vTreeIdx].vTreeFlag = -1;
|
||
for (int j = 0; j < (int)neighbours.size(); j++)
|
||
{
|
||
int idx = neighbours[j];
|
||
if (bestIdx != idx)
|
||
treeInfo_v[idx].vTreeFlag = -1;
|
||
}
|
||
}
|
||
}
|
||
std::vector<SSG_intPair> validObjects;
|
||
for (int i = 0; i < (int)objects.size(); i++)
|
||
{
|
||
int vTreeIdx = objects[i].data_0;
|
||
if (treeInfo_v[vTreeIdx].vTreeFlag < 0)
|
||
continue;
|
||
|
||
validObjects.push_back(objects[i]);
|
||
}
|
||
|
||
//生成聚类信息，
|
||
std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
|
||
std::vector<SVzNL3DRangeD> clustersRoi3D;
|
||
for (int i = 0; i < (int)validObjects.size(); i++)
|
||
{
|
||
std::vector< SVzNL2DPoint> a_cluster;
|
||
SVzNL3DRangeD a_roi3D = { {-1, -1}, {-1, -1}, {-1, -1 } };
|
||
|
||
int vTreeIdx = validObjects[i].data_0;
|
||
int hTreeIdx = validObjects[i].data_1;
|
||
for (int m = 0; m < (int)segTrees_v[vTreeIdx].treeNodes.size(); m++)
|
||
{
|
||
SWD_segFeature& a_seg = segTrees_v[vTreeIdx].treeNodes[m];
|
||
if (scanLinesInput[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx == 0)
|
||
{
|
||
scanLinesInput[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
|
||
SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.endPtIdx };
|
||
a_cluster.push_back(a_pos);
|
||
_updateRoi3D(a_roi3D, scanLinesInput[a_seg.lineIdx][a_seg.endPtIdx].pt3D);
|
||
}
|
||
if (scanLinesInput[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx == 0)
|
||
{
|
||
scanLinesInput[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
|
||
SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.startPtIdx };
|
||
a_cluster.push_back(a_pos);
|
||
_updateRoi3D(a_roi3D, scanLinesInput[a_seg.lineIdx][a_seg.startPtIdx].pt3D);
|
||
}
|
||
}
|
||
for (int m = 0; m < (int)segTrees_h[hTreeIdx].treeNodes.size(); m++)
|
||
{
|
||
SWD_segFeature& a_seg = segTrees_h[hTreeIdx].treeNodes[m];
|
||
if (scanLinesInput[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx == 0)
|
||
{
|
||
scanLinesInput[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
|
||
SVzNL2DPoint a_pos = { a_seg.startPtIdx , a_seg.lineIdx };
|
||
a_cluster.push_back(a_pos);
|
||
_updateRoi3D(a_roi3D, scanLinesInput[a_seg.startPtIdx][a_seg.lineIdx].pt3D);
|
||
}
|
||
if (scanLinesInput[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx == 0)
|
||
{
|
||
scanLinesInput[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
|
||
SVzNL2DPoint a_pos = { a_seg.endPtIdx , a_seg.lineIdx };
|
||
a_cluster.push_back(a_pos);
|
||
_updateRoi3D(a_roi3D, scanLinesInput[a_seg.endPtIdx][a_seg.lineIdx].pt3D);
|
||
}
|
||
}
|
||
clusters.push_back(a_cluster);
|
||
clustersRoi3D.push_back(a_roi3D);
|
||
}
|
||
|
||
//聚类结果分析
|
||
std::vector<int> validCluserIndexing;
|
||
int clusterSize = (int)clusters.size();
|
||
for (int i = 0; i < clusterSize; i++)
|
||
{
|
||
SVzNL3DRangeD& a_roi = clustersRoi3D[i];
|
||
double L = a_roi.xRange.max - a_roi.xRange.min;
|
||
double W = a_roi.yRange.max - a_roi.yRange.min;
|
||
if ((L > workpiecePara.holeDiameter * 0.5) && (L < workpiecePara.holeDiameter * 2.5) &&
|
||
(W > workpiecePara.holeDiameter * 0.5) && (W < workpiecePara.holeDiameter * 2.5))
|
||
validCluserIndexing.push_back(i);
|
||
}
|
||
//生成结果
|
||
std::vector< SWD_HoleInfo> holes;
|
||
int objectSize = (int)validCluserIndexing.size();
|
||
for (int objIdx = 0; objIdx < objectSize; objIdx++)
|
||
{
|
||
std::vector<SVzNL3DPoint> pointArray;
|
||
int clusterIdx = validCluserIndexing[objIdx];
|
||
//取cluster上的点
|
||
int clusterPtSize = (int)clusters[clusterIdx].size();
|
||
double minZ = DBL_MAX;
|
||
for (int i = 0; i < clusterPtSize; i++)
|
||
{
|
||
SVzNL2DPoint a_pos = clusters[clusterIdx][i];
|
||
int lineIdx = a_pos.x;
|
||
int ptIdx = a_pos.y;
|
||
SVzNL3DPoint a_pt3d = scanLines[lineIdx][ptIdx].pt3D;
|
||
if (minZ > a_pt3d.z)
|
||
minZ = a_pt3d.z;
|
||
pointArray.push_back(a_pt3d);
|
||
}
|
||
//圆拟合
|
||
SVzNL3DPoint center;
|
||
double radius;
|
||
double err = fitCircleByLeastSquare(pointArray, center, radius);
|
||
center.z = minZ;
|
||
SWD_HoleInfo a_hole;
|
||
a_hole.center = { center.x, center.y, center.z };
|
||
a_hole.radius = radius;
|
||
holes.push_back(a_hole);
|
||
}
|
||
//分割
|
||
//方法：先搜索与W最接近的点，然后条件搜索（垂直）与L最接近的点
|
||
double distDeviation = 5.0; //距离搜索的合格门限。小于此距离，认为搜索到的目标为有效
|
||
std::vector< WD_workpieceInfo> allWorkpiece;
|
||
for (int objIdx = 0; objIdx < objectSize; objIdx++)
|
||
{
|
||
if (holes[objIdx].radius < 0)
|
||
continue;
|
||
|
||
holes[objIdx].radius = -1;
|
||
SWD_HoleInfo& p0 = holes[objIdx];
|
||
int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation);
|
||
if (idx1 < 0)
|
||
continue;
|
||
|
||
SVzNLRangeD angleRange = { 85, 95 }; //垂直，5度范围
|
||
SWD_HoleInfo& p1 = holes[idx1];
|
||
//搜索最接近distance且角度为angle的目标, 以角度为优先
|
||
int idx2 = angleConditionDistanceSearch(
|
||
p0.center, p1.center,
|
||
holes,
|
||
workpiecePara.holeDist_L, distDeviation,
|
||
angleRange);
|
||
if (idx2 < 0)
|
||
continue;
|
||
|
||
SWD_HoleInfo& p2 = holes[idx2];
|
||
//搜索最接近distance且角度为angle的目标, 以角度为优先
|
||
int idx3 = angleConditionDistanceSearch(
|
||
p1.center, p0.center,
|
||
holes,
|
||
workpiecePara.holeDist_L, distDeviation,
|
||
angleRange);
|
||
if (idx3 < 0)
|
||
continue;
|
||
SWD_HoleInfo& p3 = holes[idx3];
|
||
p1.radius = -1;
|
||
p2.radius = -1;
|
||
p3.radius = -1;
|
||
|
||
//重新计算Z值。因为沉孔的原因，Z值会不准确。取四条边的中点处的Z值的均值作为整个的Z值
|
||
SVzNL3DPoint center_p0p1 = { (p0.center.x + p1.center.x) / 2,(p0.center.y + p1.center.y) / 2, (p0.center.z + p1.center.z) / 2 };
|
||
SVzNL3DPoint center_p0p2 = { (p0.center.x + p2.center.x) / 2,(p0.center.y + p2.center.y) / 2, (p0.center.z + p2.center.z) / 2 };
|
||
SVzNL3DPoint center_p1p3 = { (p1.center.x + p3.center.x) / 2,(p1.center.y + p3.center.y) / 2, (p1.center.z + p3.center.z) / 2 };
|
||
SVzNL3DPoint center_p2p3 = { (p2.center.x + p3.center.x) / 2,(p2.center.y + p3.center.y) / 2, (p2.center.z + p3.center.z) / 2 };
|
||
double rectR = 5.0;
|
||
double z1 = _getMeanZ(quantiValue, center_p0p1, roi2D, rectR);
|
||
double z2 = _getMeanZ(quantiValue, center_p0p2, roi2D, rectR);
|
||
double z3 = _getMeanZ(quantiValue, center_p1p3, roi2D, rectR);
|
||
double z4 = _getMeanZ(quantiValue, center_p2p3, roi2D, rectR);
|
||
|
||
p0.center.z = (z1 + z2) / 2;
|
||
p1.center.z = (z1 + z3) / 2;
|
||
p2.center.z = (z2 + z4) / 2;
|
||
p3.center.z = (z3 + z4) / 2;
|
||
WD_workpieceInfo a_workpiece;
|
||
a_workpiece.workpieceType = workpiecePara.workpieceType;
|
||
a_workpiece.holes.push_back(p0.center);
|
||
a_workpiece.holes.push_back(p1.center);
|
||
a_workpiece.holes.push_back(p2.center);
|
||
a_workpiece.holes.push_back(p3.center);
|
||
|
||
for (int m = 0; m < 4; m++)
|
||
{
|
||
SVzNL3DPoint a_pt = a_workpiece.holes[m];
|
||
a_pt.z = a_pt.z + 20; //法向，因为做过地面高平，所以法向只在z向
|
||
a_workpiece.holesDir.push_back(a_pt);
|
||
}
|
||
a_workpiece.center = { (p0.center.x + p1.center.x + p2.center.x + p3.center.x) / 4,
|
||
(p0.center.y + p1.center.y + p2.center.y + p3.center.y) / 4,
|
||
(z1 + z2 + z3 + z4) / 4 };
|
||
|
||
SVzNL3DPoint y_dir;
|
||
if (p0.center.x < p1.center.x)
|
||
y_dir = { p1.center.x - p0.center.x, p1.center.y - p0.center.y, 0 };
|
||
else
|
||
y_dir = { p0.center.x - p1.center.x, p0.center.y - p1.center.y, 0 };
|
||
double modLen = sqrt(pow(y_dir.x, 2) + pow(y_dir.y, 2));
|
||
y_dir = { y_dir.x / modLen, y_dir.y / modLen, 0 };
|
||
a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z };
|
||
a_workpiece.z_dir = { a_workpiece.center.x, a_workpiece.center.y, a_workpiece.center.z + 20 };
|
||
|
||
allWorkpiece.push_back(a_workpiece);
|
||
}
|
||
int workpieceNum = (int)allWorkpiece.size();
|
||
if (workpieceNum == 0)
|
||
return;
|
||
|
||
//排序
|
||
//z方向排序
|
||
std::vector< WD_workpieceInfo> zSortWorkpiece;
|
||
double minZ = allWorkpiece[0].center.z;
|
||
for (int i = 1; i < workpieceNum; i++)
|
||
{
|
||
if (minZ > allWorkpiece[i].center.z)
|
||
minZ = allWorkpiece[i].center.z;
|
||
}
|
||
double topLayerTh = minZ + workpiecePara.H / 2;
|
||
for (int i = 0; i < workpieceNum; i++)
|
||
{
|
||
if (allWorkpiece[i].center.z < topLayerTh)
|
||
zSortWorkpiece.push_back(allWorkpiece[i]);
|
||
}
|
||
//水平方向排序
|
||
while (zSortWorkpiece.size() > 0)
|
||
{
|
||
std::vector< WD_workpieceInfo> firstLine;
|
||
std::vector< WD_workpieceInfo> restWorkpiece;
|
||
_getYTopLine(zSortWorkpiece, firstLine, restWorkpiece, workpiecePara.yLen / 2);
|
||
workpiecePositioning.insert(workpiecePositioning.end(), firstLine.begin(), firstLine.end());
|
||
zSortWorkpiece.clear();
|
||
zSortWorkpiece.insert(zSortWorkpiece.end(), restWorkpiece.begin(), restWorkpiece.end());
|
||
}
|
||
|
||
//旋转回去
|
||
workpieceNum = (int)workpiecePositioning.size();
|
||
for (int i = 0; i < workpieceNum; i++)
|
||
{
|
||
SVzNL3DPoint rpt;
|
||
rpt = _ptRotate(workpiecePositioning[i].center, groundCalibPara.invRMatrix);
|
||
workpiecePositioning[i].center = rpt;
|
||
rpt = _ptRotate(workpiecePositioning[i].y_dir, groundCalibPara.invRMatrix);
|
||
workpiecePositioning[i].y_dir = rpt;
|
||
rpt = _ptRotate(workpiecePositioning[i].z_dir, groundCalibPara.invRMatrix);
|
||
workpiecePositioning[i].z_dir = rpt;
|
||
for (int j = 0, j_max = (int)workpiecePositioning[i].holes.size(); j < j_max; j++)
|
||
{
|
||
rpt = _ptRotate(workpiecePositioning[i].holes[j], groundCalibPara.invRMatrix);
|
||
workpiecePositioning[i].holes[j] = rpt;
|
||
rpt = _ptRotate(workpiecePositioning[i].holesDir[j], groundCalibPara.invRMatrix);
|
||
workpiecePositioning[i].holesDir[j] = rpt;
|
||
}
|
||
|
||
SVzNL3DPoint vector_z = { workpiecePositioning[i].z_dir.x - workpiecePositioning[i].center.x,
|
||
workpiecePositioning[i].z_dir.y - workpiecePositioning[i].center.y,
|
||
workpiecePositioning[i].z_dir.z - workpiecePositioning[i].center.z };
|
||
SVzNL3DPoint vector_y = { workpiecePositioning[i].y_dir.x - workpiecePositioning[i].center.x,
|
||
workpiecePositioning[i].y_dir.y - workpiecePositioning[i].center.y,
|
||
workpiecePositioning[i].y_dir.z - workpiecePositioning[i].center.z };
|
||
double mod_vz = sqrt(pow(vector_z.x, 2) + pow(vector_z.y, 2) + pow(vector_z.z, 2));
|
||
vector_z = { vector_z.x / mod_vz, vector_z.y / mod_vz, vector_z.z / mod_vz }; //归一化
|
||
double mod_vy = sqrt(pow(vector_y.x, 2) + pow(vector_y.y, 2) + pow(vector_y.z, 2));
|
||
vector_y = { vector_y.x / mod_vy, vector_y.y / mod_vy, vector_y.z / mod_vy }; //归一化
|
||
//叉乘出vector_x
|
||
SVzNL3DPoint vector_x;
|
||
vector_x.x = vector_y.y * vector_z.z - vector_z.y * vector_y.z;
|
||
vector_x.y = vector_y.z * vector_z.x - vector_z.z * vector_y.x;
|
||
vector_x.z = vector_y.x * vector_z.y - vector_z.x * vector_y.y;
|
||
workpiecePositioning[i].x_dir = vector_x;
|
||
workpiecePositioning[i].y_dir = vector_y;
|
||
workpiecePositioning[i].z_dir = vector_z;
|
||
#if 0
|
||
//得到旋转矩阵
|
||
double R[3][3];
|
||
R[0][0] = vector_x.x;
|
||
R[1][0] = vector_x.y;
|
||
R[2][0] = vector_x.z;
|
||
|
||
R[0][1] = vector_y.x;
|
||
R[1][1] = vector_y.y;
|
||
R[2][1] = vector_y.z;
|
||
|
||
R[0][2] = vector_z.x;
|
||
R[1][2] = vector_z.y;
|
||
R[2][2] = vector_z.z;
|
||
SSG_EulerAngles eulerAngle = rotationMatrixToEulerZYX(R);
|
||
workpiecePositioning[i].workpiecePose = eulerAngle;
|
||
#endif
|
||
}
|
||
return;
|
||
|
||
} |