2167 lines
58 KiB
C++
2167 lines
58 KiB
C++
#include <vector>
|
||
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include "hybridPosePositioning_Export.h"
|
||
#include <opencv2/opencv.hpp>
|
||
#include <limits>
|
||
|
||
#define _DEBUG_OUTPUT
|
||
|
||
//version 1.0.0 : base version release to customer
|
||
//version 1.1.0 : 海瑞马码垛规划版本
|
||
//version 1.2.0 : 添加了海瑞马锥形工件定位版本（不包含二次定位）
|
||
//version 1.3.0 : 添加了海瑞马转子钢芯定位
|
||
std::string m_strVersion = "HybridPositioning 1.3.0";
|
||
const char* wd_hybridPositioningVersion(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);
|
||
}
|
||
|
||
SVzNL3DPosition _computeMinZPoint(std::vector<SVzNL3DPosition>& points)
|
||
{
|
||
SVzNL3DPosition peak;
|
||
peak.nPointIdx = 0;
|
||
peak.pt3D = { 0.0, 0.0, 0.0 };
|
||
for (int i = 0; i < (int)points.size(); i++)
|
||
{
|
||
if (points[i].pt3D.z < 1e-4)
|
||
continue;
|
||
|
||
if (peak.pt3D.z < 1e-4)
|
||
peak = points[i];
|
||
else if (peak.pt3D.z > points[i].pt3D.z)
|
||
peak = points[i];
|
||
|
||
}
|
||
return peak;
|
||
}
|
||
|
||
SVzNL3DPoint _computeCentroid(std::vector<SVzNL3DPosition>& points)
|
||
{
|
||
SVzNL3DPoint centroid = { 0.0, 0.0, 0.0 };
|
||
int counter = 0;
|
||
for (int i = 0; i < (int)points.size(); i++)
|
||
{
|
||
if (points[i].pt3D.z < 1e-4)
|
||
continue;
|
||
|
||
counter++;
|
||
centroid.x += points[i].pt3D.x;
|
||
centroid.y += points[i].pt3D.y;
|
||
centroid.z += points[i].pt3D.z;
|
||
}
|
||
centroid.x = centroid.x / counter;
|
||
centroid.y = centroid.y / counter;
|
||
centroid.z = centroid.z / counter;
|
||
return centroid;
|
||
}
|
||
|
||
int _get2DRegion(SVzNLPositionD& a_pt2D3D, std::vector<WD_objArea2D>& objROIs)
|
||
{
|
||
for (int i = 0; i < (int)objROIs.size(); i++)
|
||
{
|
||
if ((a_pt2D3D.ptLeft2D.x >= objROIs[i].roi.left) && (a_pt2D3D.ptLeft2D.x <= objROIs[i].roi.right) &&
|
||
(a_pt2D3D.ptLeft2D.y >= objROIs[i].roi.top) && (a_pt2D3D.ptLeft2D.y <= objROIs[i].roi.bottom))
|
||
return i;
|
||
}
|
||
return -1;
|
||
}
|
||
|
||
bool _compareByZValue(SVzNL3DPosition& a, SVzNL3DPosition& b)
|
||
{
|
||
return a.pt3D.z < b.pt3D.z;
|
||
}
|
||
|
||
//生成圆周扫描数据
|
||
void _genPolarScanData(
|
||
std::vector<SVzNL3DPosition>& points,
|
||
const double angleScale,
|
||
const SVzNL3DPoint polarCener,
|
||
std::vector<std::vector<SWD_polarPt>>& polarScanData)
|
||
{
|
||
int polarLines = (int)(360.0 / angleScale + 0.5);
|
||
polarScanData.resize(polarLines);
|
||
|
||
int dataSize = (int)points.size();
|
||
for (int i = 0; i < dataSize; i++)
|
||
{
|
||
int line = points[i].nPointIdx >> 16;
|
||
int ptIdx = points[i].nPointIdx & 0x0000FFFF;
|
||
SVzNL3DPoint& a_pt = points[i].pt3D;
|
||
|
||
double angle = atan2(a_pt.y - polarCener.y, a_pt.x - polarCener.x);
|
||
angle = (angle / PI) * 180 + 180.0;
|
||
double R = sqrt(pow(a_pt.y - polarCener.y, 2) + pow(a_pt.x - polarCener.x, 2));
|
||
|
||
int angleLine = (int)(angle / angleScale + 0.5);
|
||
angleLine = angleLine % (int)polarScanData.size();
|
||
|
||
SWD_polarPt a_polarPt;
|
||
a_polarPt.lineIdx = line;
|
||
a_polarPt.ptIdx = ptIdx;
|
||
a_polarPt.x = a_pt.x;
|
||
a_polarPt.y = a_pt.y;
|
||
a_polarPt.z = a_pt.z;
|
||
a_polarPt.R = R;
|
||
a_polarPt.angle = angle;
|
||
polarScanData[angleLine].push_back(a_polarPt);
|
||
}
|
||
}
|
||
|
||
//生成圆周扫描数据, 同心圆环结构
|
||
void _genPolarScanData_2(
|
||
std::vector<SVzNL3DPosition>& points,
|
||
const double radiusScale,
|
||
const SVzNL3DPoint polarCener,
|
||
std::vector<std::vector<SWD_polarPt>>& polarScanData)
|
||
{
|
||
std::vector< SWD_polarPt> polarPoints;
|
||
int dataSize = (int)points.size();
|
||
double rMax = 0;
|
||
for (int i = 0; i < dataSize; i++)
|
||
{
|
||
int line = points[i].nPointIdx >> 16;
|
||
int ptIdx = points[i].nPointIdx & 0x0000FFFF;
|
||
SVzNL3DPoint& a_pt = points[i].pt3D;
|
||
|
||
double angle = atan2(a_pt.y - polarCener.y, a_pt.x - polarCener.x);
|
||
angle = (angle / PI) * 180 + 180.0;
|
||
double R = sqrt(pow(a_pt.y - polarCener.y, 2) + pow(a_pt.x - polarCener.x, 2));
|
||
rMax = rMax < R ? R : rMax;
|
||
|
||
SWD_polarPt a_polarPt;
|
||
a_polarPt.lineIdx = line;
|
||
a_polarPt.ptIdx = ptIdx;
|
||
a_polarPt.x = a_pt.x;
|
||
a_polarPt.y = a_pt.y;
|
||
a_polarPt.z = a_pt.z;
|
||
a_polarPt.R = R;
|
||
a_polarPt.angle = angle;
|
||
|
||
polarPoints.push_back(a_polarPt);
|
||
}
|
||
|
||
int circleNum = (int)(rMax / radiusScale) + 1;
|
||
polarScanData.resize(circleNum);
|
||
for (int i = 0; i < (int)polarPoints.size(); i++)
|
||
{
|
||
double r = polarPoints[i].R;
|
||
int idx = r / radiusScale;
|
||
polarScanData[idx].push_back(polarPoints[i]);
|
||
}
|
||
}
|
||
|
||
bool compareByPolarScanR(const SWD_polarPt& a, const SWD_polarPt& b) {
|
||
return a.R < b.R;
|
||
}
|
||
bool compareByPolarScanAngle(const SWD_polarPt& a, const SWD_polarPt& b) {
|
||
return a.angle < b.angle;
|
||
}
|
||
|
||
typedef struct
|
||
{
|
||
int hScanFlag; //0: 垂直扫描; 1-水平扫描
|
||
int lineIdx;
|
||
SSG_RUN seg;
|
||
int segValidPtNum;
|
||
double segMeanZ;
|
||
int LRFlag; //0:left; 1:right
|
||
SVzNL3DPosition outsideEdgePoint; //外侧端点
|
||
SVzNL3DPosition centerPoint;
|
||
}SWD_indexingSeg;
|
||
|
||
void _seedSegClustering_speedUp(
|
||
std::vector< SWD_indexingSeg>& a_cluster,
|
||
std::vector< SWD_indexingSeg>& segs,
|
||
std::vector<int>& flags,
|
||
double clusterDist,
|
||
int distType, //0 - 2d distance; 1- 3d distance
|
||
std::vector<std::vector<int>>& indexing2D,
|
||
int lineNum, int ptSize, int clusterCheckWin) //搜索窗口
|
||
{
|
||
int i = 0;
|
||
while (1)
|
||
{
|
||
if (i >= a_cluster.size())
|
||
break;
|
||
|
||
SWD_indexingSeg& a_seed = a_cluster[i];
|
||
int seed_line, seed_ptIdx;
|
||
if (a_seed.hScanFlag == 0) //垂直
|
||
{
|
||
seed_line = a_seed.lineIdx;
|
||
seed_ptIdx = a_seed.centerPoint.nPointIdx;
|
||
}
|
||
else
|
||
{
|
||
seed_line = a_seed.centerPoint.nPointIdx;
|
||
seed_ptIdx = a_seed.lineIdx;
|
||
}
|
||
for (int line = seed_line - clusterCheckWin; line <= seed_line + clusterCheckWin; line++)
|
||
{
|
||
if ((line >= 0) && (line < lineNum))
|
||
{
|
||
for (int ptIdx = seed_ptIdx - clusterCheckWin; ptIdx <= seed_ptIdx + clusterCheckWin; ptIdx++)
|
||
{
|
||
if ((ptIdx >= 0) && (ptIdx < ptSize))
|
||
{
|
||
int backIdx = indexing2D[line][ptIdx];
|
||
if (backIdx < 0)
|
||
continue;
|
||
if (flags[backIdx] < 0)
|
||
continue;
|
||
double dist;
|
||
if (0 == distType)
|
||
dist = sqrt(pow(a_seed.centerPoint.pt3D.x - segs[backIdx].centerPoint.pt3D.x, 2) + pow(a_seed.centerPoint.pt3D.y - segs[backIdx].centerPoint.pt3D.y, 2));
|
||
else
|
||
dist = sqrt(pow(a_seed.centerPoint.pt3D.x - segs[backIdx].centerPoint.pt3D.x, 2) + pow(a_seed.centerPoint.pt3D.y - segs[backIdx].centerPoint.pt3D.y, 2) + pow(a_seed.centerPoint.pt3D.z - segs[backIdx].centerPoint.pt3D.z, 2));
|
||
if (dist < clusterDist)
|
||
{
|
||
a_cluster.push_back(segs[backIdx]);
|
||
flags[backIdx] = -1;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
i++;
|
||
}
|
||
return;
|
||
}
|
||
|
||
void _segsClustering_speedUp(
|
||
std::vector< SWD_indexingSeg>& segs,
|
||
int lineNum, int linePtSize, int clusterCheckWin, //搜索窗口
|
||
double clusterDist,
|
||
int distType, //0 - 2d distance; 1- 3d distance
|
||
std::vector<std::vector< SWD_indexingSeg>>& objClusters //result
|
||
)
|
||
{
|
||
int segSize = (int)segs.size();
|
||
if (segSize == 0)
|
||
return;
|
||
|
||
std::vector<std::vector<int>> indexing2D;
|
||
indexing2D.resize(lineNum);
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
indexing2D[i].resize(linePtSize);
|
||
std::fill(indexing2D[i].begin(), indexing2D[i].end(), -1);
|
||
}
|
||
for (int i = 0; i < segSize; i++)
|
||
{
|
||
SWD_indexingSeg& a_seg = segs[i];
|
||
int line, ptIdx;
|
||
if (a_seg.hScanFlag == 0) //垂直
|
||
{
|
||
line = a_seg.lineIdx;
|
||
ptIdx = a_seg.centerPoint.nPointIdx;
|
||
}
|
||
else
|
||
{
|
||
line = a_seg.centerPoint.nPointIdx;
|
||
ptIdx = a_seg.lineIdx;
|
||
}
|
||
indexing2D[line][ptIdx] = i;
|
||
}
|
||
|
||
std::vector<int> flags;
|
||
flags.resize(segSize);
|
||
std::fill(flags.begin(), flags.end(), 0);
|
||
|
||
int idx = 0;
|
||
while (idx < (int)segs.size())
|
||
{
|
||
SWD_indexingSeg& a_seg = segs[idx];
|
||
if (flags[idx] >= 0)
|
||
{
|
||
flags[idx] = -1;//防止重复被计算
|
||
//新建一个cluster
|
||
std::vector< SWD_indexingSeg> a_cluster;
|
||
a_cluster.push_back(a_seg);
|
||
//pts[0].nPointIdx = -1;
|
||
_seedSegClustering_speedUp(
|
||
a_cluster,
|
||
segs,
|
||
flags,
|
||
clusterDist,
|
||
distType,
|
||
indexing2D,
|
||
lineNum, linePtSize, clusterCheckWin);
|
||
if (a_cluster.size() >= 5)
|
||
objClusters.push_back(a_cluster); //保存一个聚类
|
||
}
|
||
idx++;
|
||
}
|
||
return;
|
||
}
|
||
|
||
|
||
WD_workpieceInfo _computeWorkpiecePose(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const SWDScanPosition centerPosition,
|
||
const SWD_Cylinder workpieceParam,
|
||
std::vector<SWDScanPosition>& outsideEdgePtIndice,
|
||
std::vector<SWDScanPosition>& centerPtIndice)
|
||
{
|
||
WD_workpieceInfo a_pose;
|
||
memset(&a_pose, 0, sizeof(WD_workpieceInfo));
|
||
|
||
int lineNum = (int)scanLines.size();
|
||
int linePtNum = (int)scanLines[0].size();
|
||
|
||
std::vector<std::vector<SVzNL3DPosition>> hLines;
|
||
hLines.resize(linePtNum);
|
||
for (int i = 0; i < linePtNum; i++)
|
||
hLines[i].resize(lineNum);
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
hLines[j][line] = scanLines[line][j];
|
||
hLines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
||
hLines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
||
}
|
||
}
|
||
|
||
//过滤掉凹段
|
||
std::vector<std::vector<int>> flags;
|
||
flags.resize(lineNum);
|
||
for (int i = 0; i < lineNum; i++)
|
||
flags[i].resize(linePtNum);
|
||
|
||
//内部参数
|
||
double topSeg_minWidth = 5.0; //
|
||
double topSeg_maxWidth = 20.0;
|
||
|
||
SSG_lineSegParam filterSegPara;
|
||
filterSegPara.distScale = 0;
|
||
filterSegPara.segGapTh_y = topSeg_maxWidth;
|
||
filterSegPara.segGapTh_z = workpieceParam.height / 2;
|
||
|
||
//垂直
|
||
std::vector<std::vector<SWD_indexingSeg>> vScanLineSegs;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 83)
|
||
int kkk = 1;
|
||
//分段
|
||
std::vector<SSG_RUN> simpleSegs;
|
||
wd_getLineDataIntervals(scanLines[line], filterSegPara, simpleSegs);
|
||
|
||
//过滤掉凹段
|
||
std::vector<int> filterIndice;
|
||
//若一个段首尾都比相邻段低，视为凹陷段
|
||
for (int i = 1; i < (int)simpleSegs.size() - 1; i++)
|
||
{
|
||
int tail_1 = simpleSegs[i - 1].start + simpleSegs[i - 1].len - 1;
|
||
int head_1 = simpleSegs[i].start;
|
||
int tail_2 = simpleSegs[i].start + simpleSegs[i].len - 1;
|
||
int head_2 = simpleSegs[i + 1].start;
|
||
double z_diff_1 = scanLines[line][head_1].pt3D.z - scanLines[line][tail_1].pt3D.z;
|
||
double z_diff_2 = scanLines[line][tail_2].pt3D.z - scanLines[line][head_2].pt3D.z;
|
||
if ((z_diff_1 > filterSegPara.segGapTh_z) && (z_diff_2 > filterSegPara.segGapTh_z))
|
||
filterIndice.push_back(i);
|
||
}
|
||
for (int m = 0; m < (int)filterIndice.size(); m++)
|
||
{
|
||
int idx = filterIndice[m];
|
||
int offset = simpleSegs[idx].start;
|
||
for (int i = 0; i < simpleSegs[idx].len; i++)
|
||
flags[line][offset + i] = 1;
|
||
simpleSegs[idx].start = -1; //invalid
|
||
}
|
||
//生成扩展信息
|
||
std::vector<SWD_indexingSeg> exSegs;
|
||
for (int m = 0; m < (int)simpleSegs.size(); m++)
|
||
{
|
||
if (simpleSegs[m].start < 0)
|
||
continue;
|
||
|
||
int start = simpleSegs[m].start;
|
||
int end = simpleSegs[m].start + simpleSegs[m].len - 1;
|
||
double meanZ = 0;
|
||
int size = 0;
|
||
int namedCenter = (start + end) / 2;
|
||
int realCenter = -1;
|
||
int minDist = -1;
|
||
for (int n = start; n <= end; n++)
|
||
{
|
||
if (scanLines[line][n].pt3D.z > 1e-4)
|
||
{
|
||
meanZ += scanLines[line][n].pt3D.z;
|
||
size++;
|
||
|
||
int nDist = namedCenter > n ? (namedCenter - n) : (n - namedCenter); //距离名义中心点的距离
|
||
if (minDist < 0)
|
||
{
|
||
minDist = nDist;
|
||
realCenter = n;
|
||
}
|
||
else if (minDist > nDist)
|
||
{
|
||
minDist = nDist;
|
||
realCenter = n;
|
||
}
|
||
}
|
||
}
|
||
if (size > 0)
|
||
{
|
||
SWD_indexingSeg a_seg;
|
||
memset(&a_seg, 0, sizeof(SWD_indexingSeg));
|
||
a_seg.centerPoint.nPointIdx = realCenter;
|
||
a_seg.centerPoint.pt3D = scanLines[line][realCenter].pt3D;
|
||
a_seg.lineIdx = line;
|
||
a_seg.seg = simpleSegs[m];
|
||
a_seg.segMeanZ = meanZ / size;
|
||
a_seg.segValidPtNum = size;
|
||
exSegs.push_back(a_seg);
|
||
}
|
||
}
|
||
if (exSegs.size() == 0)
|
||
return a_pose;
|
||
//去除低的段：如果两个段的meanZ相差达到工件高度的一半，视为低的段
|
||
int highestSeg = 0;
|
||
for (int m = 1; m < (int)exSegs.size(); m++)
|
||
{
|
||
if (exSegs[highestSeg].segMeanZ > exSegs[m].segMeanZ)
|
||
highestSeg = m;
|
||
}
|
||
|
||
double highestZ = exSegs[highestSeg].segMeanZ;
|
||
std::vector<SWD_indexingSeg> validExSegs;
|
||
for (int m = 0; m < (int)exSegs.size(); m++)
|
||
{
|
||
double zDiff = exSegs[m].segMeanZ - highestZ;
|
||
if (zDiff < workpieceParam.height / 2)
|
||
{
|
||
validExSegs.push_back(exSegs[m]);
|
||
}
|
||
}
|
||
vScanLineSegs.push_back(validExSegs);
|
||
}
|
||
|
||
//水平扫描
|
||
std::vector<std::vector<SWD_indexingSeg>> hScanLineSegs;
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
//分段
|
||
std::vector<SSG_RUN> simpleSegs;
|
||
wd_getLineDataIntervals(hLines[line], filterSegPara, simpleSegs);
|
||
|
||
//过滤掉凹段
|
||
std::vector<int> filterIndice;
|
||
//若一个段首尾都比相邻段低，视为凹陷段
|
||
for (int i = 1; i < (int)simpleSegs.size() - 1; i++)
|
||
{
|
||
int tail_1 = simpleSegs[i - 1].start + simpleSegs[i - 1].len - 1;
|
||
int head_1 = simpleSegs[i].start;
|
||
int tail_2 = simpleSegs[i].start + simpleSegs[i].len - 1;
|
||
int head_2 = simpleSegs[i + 1].start;
|
||
double z_diff_1 = hLines[line][head_1].pt3D.z - hLines[line][tail_1].pt3D.z;
|
||
double z_diff_2 = hLines[line][tail_2].pt3D.z - hLines[line][head_2].pt3D.z;
|
||
if ((z_diff_1 > filterSegPara.segGapTh_z) && (z_diff_2 > filterSegPara.segGapTh_z))
|
||
filterIndice.push_back(i);
|
||
}
|
||
for (int m = 0; m < (int)filterIndice.size(); m++)
|
||
{
|
||
int idx = filterIndice[m];
|
||
int offset = simpleSegs[idx].start;
|
||
for (int i = 0; i < simpleSegs[idx].len; i++)
|
||
flags[offset + i][line] = 1;
|
||
simpleSegs[idx].start = -1; //invalidate
|
||
}
|
||
|
||
//生成扩展信息
|
||
std::vector<SWD_indexingSeg> exSegs;
|
||
for (int m = 0; m < (int)simpleSegs.size(); m++)
|
||
{
|
||
if (simpleSegs[m].start < 0)
|
||
continue;
|
||
|
||
int start = simpleSegs[m].start;
|
||
int end = simpleSegs[m].start + simpleSegs[m].len - 1;
|
||
double meanZ = 0;
|
||
int size = 0;
|
||
int namedCenter = (start + end) / 2;
|
||
int realCenter = -1;
|
||
int minDist = -1;
|
||
for (int n = start; n <= end; n++)
|
||
{
|
||
if (hLines[line][n].pt3D.z > 1e-4)
|
||
{
|
||
meanZ += hLines[line][n].pt3D.z;
|
||
size++;
|
||
|
||
int nDist = namedCenter > n ? (namedCenter - n) : (n - namedCenter); //距离名义中心点的距离
|
||
if (minDist < 0)
|
||
{
|
||
minDist = nDist;
|
||
realCenter = n;
|
||
}
|
||
else if (minDist > nDist)
|
||
{
|
||
minDist = nDist;
|
||
realCenter = n;
|
||
}
|
||
}
|
||
}
|
||
if (size > 0)
|
||
{
|
||
SWD_indexingSeg a_seg;
|
||
memset(&a_seg, 0, sizeof(SWD_indexingSeg));
|
||
a_seg.centerPoint.nPointIdx = realCenter;
|
||
a_seg.centerPoint.pt3D = scanLines[realCenter][line].pt3D;
|
||
a_seg.lineIdx = line;
|
||
a_seg.seg = simpleSegs[m];
|
||
a_seg.segMeanZ = meanZ / size;
|
||
a_seg.segValidPtNum = size;
|
||
exSegs.push_back(a_seg);
|
||
}
|
||
}
|
||
//去除低的段：如果两个段的meanZ相差达到工件高度的一半，视为低的段
|
||
int highestSeg = 0;
|
||
for (int m = 1; m < (int)exSegs.size(); m++)
|
||
{
|
||
if (exSegs[highestSeg].segMeanZ > exSegs[m].segMeanZ)
|
||
highestSeg = m;
|
||
}
|
||
|
||
double highestZ = exSegs[highestSeg].segMeanZ;
|
||
std::vector<SWD_indexingSeg> validExSegs;
|
||
for (int m = 0; m < (int)exSegs.size(); m++)
|
||
{
|
||
double zDiff = exSegs[m].segMeanZ - highestZ;
|
||
if (zDiff < workpieceParam.height / 2)
|
||
{
|
||
validExSegs.push_back(exSegs[m]);
|
||
}
|
||
}
|
||
hScanLineSegs.push_back(validExSegs);
|
||
}
|
||
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if (flags[line][j] > 0)
|
||
scanLines[line][j].pt3D.z = 0;
|
||
}
|
||
}
|
||
|
||
SVzNL3DPoint centerPoint = scanLines[centerPosition.lineIdx][centerPosition.ptIdx].pt3D;
|
||
std::vector< SWD_indexingSeg> segsTop;
|
||
//生成工件顶面上的点: 宽度大于一个门限（滤噪）；边界点不能是边界
|
||
//垂直扫描处理
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 83)
|
||
int kkk = 1;
|
||
int mostLeftSegId = -1; //对于垂直扫描，实际上是最上面
|
||
int mostRightSegId = -1;//对于垂直扫描，实际上是最下面
|
||
for (int i = 0; i < (int)vScanLineSegs[line].size(); i++)
|
||
{
|
||
SWD_indexingSeg& exSeg = vScanLineSegs[line][i];
|
||
|
||
int idx1 = exSeg.seg.start;
|
||
int idx2 = exSeg.seg.start + exSeg.seg.len - 1;
|
||
double width = abs(scanLines[line][idx1].pt3D.y - scanLines[line][idx2].pt3D.y);
|
||
SVzNL3DPoint segCenter = exSeg.centerPoint.pt3D;
|
||
double distToCircleCenter = sqrt(pow(centerPoint.x - segCenter.x, 2) + pow(centerPoint.y - segCenter.y, 2));
|
||
if ( (width > topSeg_minWidth) && (width < topSeg_maxWidth) && (distToCircleCenter < workpieceParam.radius))
|
||
{
|
||
if ((mostLeftSegId < 0) && (idx2 < centerPosition.ptIdx) &&(idx1 > 0 ))
|
||
mostLeftSegId = i;
|
||
|
||
if( (idx1 > centerPosition.ptIdx) && (idx2 < linePtNum - 1))
|
||
mostRightSegId = i;
|
||
}
|
||
}
|
||
if (mostLeftSegId >= 0)
|
||
{
|
||
SWD_indexingSeg& left_seg = vScanLineSegs[line][mostLeftSegId];
|
||
int edge2 = left_seg.seg.start + left_seg.seg.len - 1;
|
||
if ((edge2 < centerPosition.ptIdx) && (left_seg.seg.start > 0)) //最左侧，不能是起点（如果是起点可能连接到其他工件上)
|
||
{
|
||
int outEdgeIdx = left_seg.seg.start;
|
||
left_seg.hScanFlag = 0;
|
||
left_seg.LRFlag = 0;
|
||
left_seg.outsideEdgePoint.nPointIdx = outEdgeIdx;
|
||
left_seg.outsideEdgePoint.pt3D = scanLines[line][outEdgeIdx].pt3D;
|
||
segsTop.push_back(left_seg);
|
||
}
|
||
}
|
||
if(mostRightSegId >= 0)
|
||
{
|
||
SWD_indexingSeg& right_seg = vScanLineSegs[line][mostRightSegId];
|
||
int edge2 = right_seg.seg.start + right_seg.seg.len - 1;
|
||
if ((edge2 > centerPosition.ptIdx) && (edge2 < linePtNum -1)) //最右侧，不能是终点（如果是终点可能连接到其他工件上)
|
||
{
|
||
int outEdgeIdx = right_seg.seg.start + right_seg.seg.len - 1;
|
||
right_seg.hScanFlag = 0;
|
||
right_seg.LRFlag = 1;
|
||
right_seg.outsideEdgePoint.nPointIdx = outEdgeIdx;
|
||
right_seg.outsideEdgePoint.pt3D = scanLines[line][outEdgeIdx].pt3D;
|
||
segsTop.push_back(right_seg);
|
||
}
|
||
}
|
||
}
|
||
//水平水平处理
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
int mostLeftSegId = -1; //对于水平扫描，实际上是最左面
|
||
int mostRightSegId = -1;//对于水平扫描，实际上是最右面
|
||
for (int i = 0; i < (int)hScanLineSegs[line].size(); i++)
|
||
{
|
||
SWD_indexingSeg& a_seg = hScanLineSegs[line][i];
|
||
|
||
int idx1 = a_seg.seg.start;
|
||
int idx2 = a_seg.seg.start + a_seg.seg.len - 1;
|
||
double width = abs(hLines[line][idx1].pt3D.y - hLines[line][idx2].pt3D.y);
|
||
SVzNL3DPoint segCenter = a_seg.centerPoint.pt3D;
|
||
double distToCircleCenter = sqrt(pow(centerPoint.x - segCenter.x, 2) + pow(centerPoint.y - segCenter.y, 2));
|
||
if ( (width > topSeg_minWidth) && (width < topSeg_maxWidth) && (distToCircleCenter < workpieceParam.radius))
|
||
{
|
||
if ((mostLeftSegId < 0) && (idx2 < centerPosition.lineIdx) && (idx1 > 0))
|
||
mostLeftSegId = i;
|
||
|
||
if( (idx1 > centerPosition.lineIdx) && (idx2 < lineNum - 1))
|
||
mostRightSegId = i;
|
||
}
|
||
}
|
||
if (mostLeftSegId >= 0)
|
||
{
|
||
SWD_indexingSeg& left_seg = hScanLineSegs[line][mostLeftSegId];
|
||
int edge2 = left_seg.seg.start + left_seg.seg.len - 1;
|
||
if ((edge2 < centerPosition.lineIdx) && (left_seg.seg.start > 0)) //最左侧，不能是起点（如果是起点可能连接到其他工件上)
|
||
{
|
||
int outEdgeIdx = left_seg.seg.start;
|
||
left_seg.hScanFlag = 1;
|
||
left_seg.LRFlag = 0;
|
||
left_seg.outsideEdgePoint.nPointIdx = outEdgeIdx;
|
||
left_seg.outsideEdgePoint.pt3D = scanLines[outEdgeIdx][line].pt3D;
|
||
segsTop.push_back(left_seg);
|
||
}
|
||
}
|
||
if (mostRightSegId >= 0)
|
||
{
|
||
SWD_indexingSeg& right_seg = hScanLineSegs[line][mostRightSegId];
|
||
int edge2 = right_seg.seg.start + right_seg.seg.len - 1;
|
||
if ((edge2 > centerPosition.lineIdx) && (edge2 < lineNum - 1)) //最右侧，不能是终点（如果是终点可能连接到其他工件上)
|
||
{
|
||
int outEdgeIdx = right_seg.seg.start + right_seg.seg.len - 1;
|
||
right_seg.hScanFlag = 1;
|
||
right_seg.LRFlag = 1;
|
||
right_seg.outsideEdgePoint.nPointIdx = outEdgeIdx;
|
||
right_seg.outsideEdgePoint.pt3D = scanLines[outEdgeIdx][line].pt3D;
|
||
segsTop.push_back(right_seg);
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
//聚类
|
||
int clusterCheckWin = 5;
|
||
double clusterDist = 5.0;
|
||
int distType = 1;
|
||
std::vector<std::vector< SWD_indexingSeg>> objClusters;
|
||
_segsClustering_speedUp(
|
||
segsTop,
|
||
lineNum, linePtNum, clusterCheckWin, //搜索窗口
|
||
clusterDist,
|
||
distType, //0 - 2d distance; 1- 3d distance
|
||
objClusters //result
|
||
);
|
||
|
||
//判断是否为合格目标
|
||
if (objClusters.size() == 0)
|
||
return a_pose;
|
||
|
||
std::vector<double> clusterMeanZ;
|
||
clusterMeanZ.resize(objClusters.size());
|
||
for (int i = 0; i < (int)objClusters.size(); i++)
|
||
{
|
||
std::vector< SWD_indexingSeg>& a_cluster = objClusters[i];
|
||
int nodeSize = (int)a_cluster.size();
|
||
double meanZ = 0;
|
||
for (int m = 0; m < nodeSize; m++)
|
||
meanZ += a_cluster[m].centerPoint.pt3D.z;
|
||
meanZ = meanZ / nodeSize;
|
||
clusterMeanZ[i] = meanZ;
|
||
}
|
||
//检测相互之间的高差
|
||
bool isValid = true;
|
||
for (int m = 0; m < (int)objClusters.size(); m++)
|
||
{
|
||
for (int n = m + 1; n < (int)objClusters.size(); n++)
|
||
{
|
||
double zDiff = abs(clusterMeanZ[m] - clusterMeanZ[n]);
|
||
if (zDiff > workpieceParam.height / 2)
|
||
{
|
||
isValid = false;
|
||
break;
|
||
}
|
||
}
|
||
if (false == isValid)
|
||
break;
|
||
}
|
||
if (false == isValid)
|
||
return a_pose;
|
||
|
||
|
||
//计算整个的meanZ, 作为工件的meanZ
|
||
double objZ = 0;
|
||
int totalNodeSize = 0;
|
||
for (int i = 0; i < (int)objClusters.size(); i++)
|
||
{
|
||
std::vector< SWD_indexingSeg>& a_cluster = objClusters[i];
|
||
int nodeSize = (int)a_cluster.size();
|
||
totalNodeSize += nodeSize;
|
||
objZ += nodeSize * clusterMeanZ[i];
|
||
}
|
||
objZ = objZ / totalNodeSize;
|
||
|
||
//使用边缘点进行圆拟合计算中心点
|
||
std::vector<SVzNL3DPoint> pointArray;
|
||
for (int i = 0; i < (int)objClusters.size(); i++)
|
||
{
|
||
std::vector< SWD_indexingSeg>& a_cluster = objClusters[i];
|
||
int nodeSize = (int)a_cluster.size();
|
||
for (int m = 0; m < nodeSize; m++)
|
||
pointArray.push_back(a_cluster[m].outsideEdgePoint.pt3D);
|
||
}
|
||
SVzNL3DPoint center;
|
||
double radius;
|
||
double err = fitCircleByLeastSquare(pointArray, center, radius);
|
||
|
||
//迭代一次
|
||
pointArray.clear();
|
||
for (int i = 0; i < (int)objClusters.size(); i++)
|
||
{
|
||
std::vector< SWD_indexingSeg>& a_cluster = objClusters[i];
|
||
int nodeSize = (int)a_cluster.size();
|
||
for (int m = 0; m < nodeSize; m++)
|
||
{
|
||
SWD_indexingSeg& a_seg = a_cluster[m];
|
||
double r1 = sqrt(pow(center.x - a_seg.outsideEdgePoint.pt3D.x, 2) + pow(center.y - a_seg.outsideEdgePoint.pt3D.y, 2));
|
||
if (r1 < workpieceParam.radius * 1.05)
|
||
{
|
||
pointArray.push_back(a_cluster[m].outsideEdgePoint.pt3D);
|
||
//生成标注需要的信息
|
||
if (a_seg.hScanFlag == 0)
|
||
{
|
||
SWDScanPosition pos_center = { a_seg.lineIdx, a_seg.centerPoint.nPointIdx };
|
||
centerPtIndice.push_back(pos_center);
|
||
SWDScanPosition pos_edge = { a_seg.lineIdx, a_seg.outsideEdgePoint.nPointIdx };
|
||
outsideEdgePtIndice.push_back(pos_edge);
|
||
}
|
||
else
|
||
{
|
||
SWDScanPosition pos_center = { a_seg.centerPoint.nPointIdx, a_seg.lineIdx };
|
||
centerPtIndice.push_back(pos_center);
|
||
SWDScanPosition pos_edge = { a_seg.outsideEdgePoint.nPointIdx, a_seg.lineIdx };
|
||
outsideEdgePtIndice.push_back(pos_edge);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
err = fitCircleByLeastSquare(pointArray, center, radius);
|
||
a_pose.center = { center.x, center.y, objZ };
|
||
a_pose.value = radius;
|
||
a_pose.z_dir = { 0.0, 0.0, 1.0 };
|
||
a_pose.y_dir = { 0.0, 0.0, 0.0 };
|
||
a_pose.x_dir = { 0.0, 0.0, 0.0 };
|
||
return a_pose;
|
||
}
|
||
|
||
typedef struct
|
||
{
|
||
std::vector<SWDScanPosition> mappings;//每个2D点可能对应多个3D点
|
||
}mapping2DTo3D;
|
||
|
||
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_HRM_RotorCorePositioning(
|
||
std::vector< std::vector<SVzNLPositionD>>& scanLinesInput,
|
||
std::vector<WD_objArea2D>& objROIs,
|
||
const SSG_planeCalibPara groundCalibPara,
|
||
const SWD_Cylinder workpieceParam, //圆柱形工件的标称半径和高度
|
||
std::vector< WD_workpieceInfo>& workpiecePositions,
|
||
#ifdef _OUTPUT_DEBUG_DATA
|
||
std::vector<std::vector<SVzNL3DPosition>>& debugScanLines,
|
||
#endif
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
|
||
if (objROIs.size() == 0)
|
||
{
|
||
*errCode = SX_ERR_ZERO_2D_OBJECTS;
|
||
return;
|
||
}
|
||
|
||
int lineNum = (int)scanLinesInput.size();
|
||
int linePtNum = (int)scanLinesInput[0].size();
|
||
int maxU = 0; //2D图像的最大Col
|
||
int maxV = 0; //2D图像的最大Row
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int ptIdx = 0; ptIdx < (int)scanLinesInput[line].size(); ptIdx++)
|
||
{
|
||
if (scanLinesInput[line][ptIdx].pt3D.z < 1e-4)
|
||
continue;
|
||
|
||
maxU = maxU < scanLinesInput[line][ptIdx].ptLeft2D.x ? scanLinesInput[line][ptIdx].ptLeft2D.x : maxU;
|
||
maxV = maxV < scanLinesInput[line][ptIdx].ptLeft2D.y ? scanLinesInput[line][ptIdx].ptLeft2D.y : maxV;
|
||
}
|
||
}
|
||
|
||
//生成图像与3d的对应表
|
||
const int imgCols = maxU+1;
|
||
const int imgRows = maxV+1;
|
||
std::vector<std::vector<mapping2DTo3D>> mappingTable; //按图像大小建立。
|
||
mappingTable.resize(imgCols); //与扫描线方向对应
|
||
for (int i = 0; i < imgCols; i++)
|
||
mappingTable[i].resize(imgRows);
|
||
|
||
std::vector<std::vector<SVzNL3DPosition>> scanLines;
|
||
scanLines.resize(lineNum);
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
scanLines[line].resize(linePtNum);
|
||
for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
|
||
{
|
||
scanLinesInput[line][ptIdx].nPointIdx = ptIdx;
|
||
scanLines[line][ptIdx].pt3D = scanLinesInput[line][ptIdx].pt3D;
|
||
|
||
if (scanLinesInput[line][ptIdx].pt3D.z < 1e-4)
|
||
continue;
|
||
|
||
SWDScanPosition a_pos;
|
||
a_pos.lineIdx = line;
|
||
a_pos.ptIdx = ptIdx;
|
||
int u = scanLinesInput[line][ptIdx].ptLeft2D.x;
|
||
int v = scanLinesInput[line][ptIdx].ptLeft2D.y;
|
||
mappingTable[u][v].mappings.push_back(a_pos);
|
||
}
|
||
}
|
||
//进行调平处理
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
//调平，去除地面
|
||
wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
|
||
}
|
||
|
||
//产生水平扫描数据
|
||
std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
|
||
scanLines_h.resize(linePtNum);
|
||
for (int i = 0; i < linePtNum; i++)
|
||
scanLines_h[i].resize(lineNum);
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
scanLines_h[j][line].nPointIdx = line;
|
||
scanLines_h[j][line].pt3D = { scanLines[line][j].pt3D.y, scanLines[line][j].pt3D.x, scanLines[line][j].pt3D.z };
|
||
}
|
||
}
|
||
|
||
//去除垂直段
|
||
SSG_cornerParam removeVertialPara;
|
||
memset(&removeVertialPara, 0, sizeof(SSG_cornerParam));
|
||
removeVertialPara.scale = 3.0;
|
||
removeVertialPara.cornerTh = 60;
|
||
|
||
std::vector<std::vector<int>> flags;
|
||
flags.resize(lineNum);
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
flags[i].resize(linePtNum);
|
||
std::fill(flags[i].begin(), flags[i].end(), 0);
|
||
}
|
||
std::vector<std::vector<int>> zVertivalFlags;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 700)
|
||
int kkk = 1;
|
||
std::vector<int> line_verticalFlags;
|
||
wd_getXYVertialFeature_dirAngleMethod(
|
||
scanLines[line],
|
||
line,
|
||
removeVertialPara,
|
||
line_verticalFlags
|
||
);
|
||
zVertivalFlags.push_back(line_verticalFlags);
|
||
|
||
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
||
{
|
||
if (line_verticalFlags[i] > 0)
|
||
flags[line][i] = 1;
|
||
}
|
||
}
|
||
|
||
std::vector<std::vector<int>> zVertivalFlags_h;
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
if (line == 1177)
|
||
int kkk = 1;
|
||
std::vector<int> line_verticalFlags;
|
||
wd_getXYVertialFeature_dirAngleMethod(
|
||
scanLines_h[line],
|
||
line,
|
||
removeVertialPara,
|
||
line_verticalFlags
|
||
);
|
||
zVertivalFlags_h.push_back(line_verticalFlags);
|
||
|
||
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
||
{
|
||
if (line_verticalFlags[i] > 0)
|
||
flags[i][line] = 1;
|
||
}
|
||
}
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if (flags[line][j] > 0)
|
||
{
|
||
scanLines[line][j].pt3D.z = 0;
|
||
scanLines_h[j][line].pt3D.z = 0;
|
||
}
|
||
}
|
||
}
|
||
//迭代一次
|
||
SSG_lineSegParam lineSegPara;
|
||
lineSegPara.distScale = 5.0;
|
||
lineSegPara.segGapTh_y = 5.0;
|
||
lineSegPara.segGapTh_z = 5.0;
|
||
const int minSegLen = 5;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
std::vector<SSG_RUN> segs;
|
||
wd_getLineDataIntervals(
|
||
scanLines[line],
|
||
lineSegPara,
|
||
segs);
|
||
for (int i = 0; i < (int)segs.size(); i++)
|
||
{
|
||
if (segs[i].len <= minSegLen)
|
||
{
|
||
int idx0 = segs[i].start;
|
||
for (int j = 0; j < segs[i].len; j++)
|
||
flags[line][idx0 + j] = 1;
|
||
}
|
||
}
|
||
}
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
std::vector<SSG_RUN> segs;
|
||
wd_getLineDataIntervals(
|
||
scanLines_h[line],
|
||
lineSegPara,
|
||
segs);
|
||
for (int i = 0; i < (int)segs.size(); i++)
|
||
{
|
||
if (segs[i].len <= minSegLen)
|
||
{
|
||
int idx0 = segs[i].start;
|
||
for (int j = 0; j < segs[i].len; j++)
|
||
flags[idx0 + j][line] = 1;
|
||
}
|
||
}
|
||
}
|
||
|
||
//标注
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
}
|
||
//将垂直线段去除
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if (flags[line][j] > 0)
|
||
scanLines[line][j].pt3D.z = 0;
|
||
}
|
||
}
|
||
|
||
int objNum = (int)objROIs.size();
|
||
std::vector<WD_workpieceInfo> allWorkpiece;
|
||
for (int idx = 0; idx < objNum; idx++)
|
||
{
|
||
WD_objArea2D& obj_roi = objROIs[idx];
|
||
double cx = (obj_roi.roi.left + obj_roi.roi.right) / 2;
|
||
double cy = (obj_roi.roi.top + obj_roi.roi.bottom) / 2;
|
||
double radius = cx - obj_roi.roi.left;
|
||
radius = radius * 1.414;
|
||
|
||
int L = (int)(cx - radius);
|
||
int R = (int)(cx + radius + 0.5);
|
||
int T = (int)(cy - radius);
|
||
int B = (int)(cy + radius + 0.5);
|
||
int nCx = (int)cx;
|
||
int nCy = (int)cy;
|
||
|
||
//统计ROI中的扫描线和PtIdx范围
|
||
SVzNLRange roiLineIndice = { INT_MAX, 0 };
|
||
SVzNLRange roiPtIndice = { INT_MAX, 0 };
|
||
for (int x = L; x <= R; x++)
|
||
{
|
||
for (int y = T; y <= B; y++)
|
||
{
|
||
mapping2DTo3D& a_mapping = mappingTable[x][y];
|
||
for (int m = 0; m < (int)a_mapping.mappings.size(); m++)
|
||
{
|
||
int lineIdx = a_mapping.mappings[m].lineIdx;
|
||
int ptIdx = a_mapping.mappings[m].ptIdx;
|
||
if (scanLines[lineIdx][ptIdx].pt3D.z > 1e-4)
|
||
{
|
||
scanLinesInput[lineIdx][ptIdx].nPointIdx &= 0xffff0000;
|
||
scanLinesInput[lineIdx][ptIdx].nPointIdx |= (idx + 1);
|
||
roiLineIndice.nMin = roiLineIndice.nMin > lineIdx ? lineIdx : roiLineIndice.nMin;
|
||
roiLineIndice.nMax = roiLineIndice.nMax < lineIdx ? lineIdx : roiLineIndice.nMax;
|
||
roiPtIndice.nMin = roiPtIndice.nMin > ptIdx ? ptIdx : roiPtIndice.nMin;
|
||
roiPtIndice.nMax = roiPtIndice.nMax < ptIdx ? ptIdx : roiPtIndice.nMax;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
//生成ROI中的扫描数据
|
||
SWDScanPosition centerPosition = { 0, 0 };
|
||
int minDist = -1;
|
||
int roiLines = roiLineIndice.nMax - roiLineIndice.nMin + 1;
|
||
int roiLinePtNum = roiPtIndice.nMax - roiPtIndice.nMin + 1;
|
||
std::vector< std::vector<SVzNL3DPosition>> roiScanLines;
|
||
roiScanLines.resize(roiLines);
|
||
for (int line = 0; line < roiLines; line++)
|
||
roiScanLines[line].resize(roiLinePtNum);
|
||
for (int x = L; x <= R; x++)
|
||
{
|
||
for (int y = T; y <= B; y++)
|
||
{
|
||
mapping2DTo3D& a_mapping = mappingTable[x][y];
|
||
for (int m = 0; m < (int)a_mapping.mappings.size(); m++)
|
||
{
|
||
int lineIdx = a_mapping.mappings[m].lineIdx;
|
||
int ptIdx = a_mapping.mappings[m].ptIdx;
|
||
if ((lineIdx == 275) && (ptIdx == 929))
|
||
int kkk = 1;
|
||
if (scanLines[lineIdx][ptIdx].pt3D.z > 1e-4)
|
||
{
|
||
scanLines[lineIdx][ptIdx].nPointIdx &= 0xffff0000;
|
||
scanLines[lineIdx][ptIdx].nPointIdx |= idx + 1;
|
||
int roiLineIdx = lineIdx - roiLineIndice.nMin;
|
||
int roiPtIdx = ptIdx - roiPtIndice.nMin;
|
||
roiScanLines[roiLineIdx][roiPtIdx].nPointIdx = 0;
|
||
roiScanLines[roiLineIdx][roiPtIdx].pt3D = scanLines[lineIdx][ptIdx].pt3D;
|
||
//搜索中心点
|
||
int dist = abs(x - nCx) + abs(y - nCy);
|
||
if (minDist < 0)
|
||
{
|
||
minDist = dist;
|
||
centerPosition = { roiLineIdx, roiPtIdx };
|
||
}
|
||
else if (minDist > dist)
|
||
{
|
||
minDist = dist;
|
||
centerPosition = { roiLineIdx, roiPtIdx };
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
//判断工件种类，计算工件姿态
|
||
std::vector<SWDScanPosition> outsideEdgePtIndice;
|
||
std::vector<SWDScanPosition> centerPtIndice;
|
||
WD_workpieceInfo a_workpiece = _computeWorkpiecePose(
|
||
roiScanLines,
|
||
centerPosition,
|
||
workpieceParam,
|
||
outsideEdgePtIndice,
|
||
centerPtIndice);
|
||
|
||
#ifdef _OUTPUT_DEBUG_DATA
|
||
if (a_workpiece.center.z > 1e-4)
|
||
{
|
||
//标注
|
||
for (int i = 0; i < (int)outsideEdgePtIndice.size(); i++)
|
||
{
|
||
int line = outsideEdgePtIndice[i].lineIdx + roiLineIndice.nMin;
|
||
int ptIdx = outsideEdgePtIndice[i].ptIdx + roiPtIndice.nMin;
|
||
scanLines[line][ptIdx].nPointIdx |= 0x10000;
|
||
}
|
||
for (int i = 0; i < (int)centerPtIndice.size(); i++)
|
||
{
|
||
int line = centerPtIndice[i].lineIdx + roiLineIndice.nMin;
|
||
int ptIdx = centerPtIndice[i].ptIdx + roiPtIndice.nMin;
|
||
scanLines[line][ptIdx].nPointIdx |= 0x20000;
|
||
}
|
||
}
|
||
#endif
|
||
|
||
if (a_workpiece.center.z > 1e-4)
|
||
allWorkpiece.push_back(a_workpiece);
|
||
|
||
}
|
||
if (allWorkpiece.size() == 0)
|
||
return;
|
||
|
||
//排序
|
||
//搜索最高目标
|
||
double highest_z = allWorkpiece[0].center.z;
|
||
for (int i = 1; i < (int)allWorkpiece.size(); i++)
|
||
highest_z = highest_z > allWorkpiece[i].center.z ? allWorkpiece[i].center.z : highest_z;
|
||
//z方向排序
|
||
std::vector< WD_workpieceInfo> zSortWorkpiece;
|
||
double topLayerTh = highest_z + workpieceParam.height * 0.75;
|
||
for (int i = 0; i < (int)allWorkpiece.size(); 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, workpieceParam.radius);
|
||
workpiecePositions.insert(workpiecePositions.end(), firstLine.begin(), firstLine.end());
|
||
zSortWorkpiece.clear();
|
||
zSortWorkpiece.insert(zSortWorkpiece.end(), restWorkpiece.begin(), restWorkpiece.end());
|
||
}
|
||
|
||
//旋转回去
|
||
int workpieceNum = (int)workpiecePositions.size();
|
||
for (int i = 0; i < workpieceNum; i++)
|
||
{
|
||
SVzNL3DPoint rpt;
|
||
rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix);
|
||
workpiecePositions[i].center = rpt;
|
||
rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix);
|
||
workpiecePositions[i].y_dir = rpt;
|
||
rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.invRMatrix);
|
||
workpiecePositions[i].z_dir = rpt;
|
||
rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix);
|
||
workpiecePositions[i].x_dir = rpt;
|
||
}
|
||
|
||
#ifdef _OUTPUT_DEBUG_DATA
|
||
debugScanLines.resize(scanLines.size());
|
||
for (int line = 0; line < (int)scanLines.size(); line++)
|
||
{
|
||
debugScanLines[line].resize(scanLines[line].size());
|
||
for (int j = 0; j < (int)scanLines[line].size(); j++)
|
||
debugScanLines[line][j] = scanLines[line][j];
|
||
}
|
||
#endif
|
||
return;
|
||
}
|
||
|
||
void wd_HRM_TaperedWorkpiecePositioning(
|
||
std::vector< std::vector<SVzNLPositionD>>& scanLinesInput,
|
||
std::vector<WD_objArea2D>& objROIs,
|
||
const SSG_planeCalibPara groundCalibPara,
|
||
std::vector< WD_workpieceInfo>& workpiecePositions,
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
|
||
if (objROIs.size() == 0)
|
||
{
|
||
*errCode = SX_ERR_ZERO_2D_OBJECTS;
|
||
return;
|
||
}
|
||
|
||
std::vector<std::vector<SVzNL3DPosition>> rgnPoints;
|
||
rgnPoints.resize(objROIs.size());
|
||
for (int line = 0; line < (int)scanLinesInput.size(); line++)
|
||
{
|
||
for (int ptIdx = 0; ptIdx < (int)scanLinesInput[line].size(); ptIdx++)
|
||
{
|
||
scanLinesInput[line][ptIdx].nPointIdx = 0; //转义使用
|
||
if (scanLinesInput[line][ptIdx].pt3D.z < 1e-4)
|
||
continue;
|
||
|
||
int rgnIdx = _get2DRegion(scanLinesInput[line][ptIdx], objROIs);
|
||
if (rgnIdx >= 0)
|
||
{
|
||
SVzNL3DPosition a_rgnPt;
|
||
a_rgnPt.nPointIdx = (line << 16) | (ptIdx & 0xffff);
|
||
a_rgnPt.pt3D = scanLinesInput[line][ptIdx].pt3D;
|
||
rgnPoints[rgnIdx].push_back(a_rgnPt);
|
||
scanLinesInput[line][ptIdx].nPointIdx = rgnIdx + 1;
|
||
}
|
||
}
|
||
}
|
||
|
||
//统计每个region的最高点
|
||
std::vector< SVzNL3DPosition> rgnPeaks;
|
||
for (int i = 0; i < (int)rgnPoints.size(); i++)
|
||
{
|
||
SVzNL3DPosition peakPoint = _computeMinZPoint(rgnPoints[i]);
|
||
SVzNL3DPosition a_peak;
|
||
a_peak.nPointIdx = i;
|
||
a_peak.pt3D = peakPoint.pt3D;
|
||
rgnPeaks.push_back(a_peak);
|
||
}
|
||
|
||
//按高度排序
|
||
std::sort(rgnPeaks.begin(), rgnPeaks.end(), _compareByZValue);
|
||
for (int i = 0; i < (int)rgnPeaks.size(); i++)
|
||
{
|
||
WD_workpieceInfo a_obj;
|
||
memset(&a_obj, 0, sizeof(WD_workpieceInfo));
|
||
a_obj.center = rgnPeaks[i].pt3D;
|
||
workpiecePositions.push_back(a_obj);
|
||
}
|
||
return;
|
||
|
||
}
|
||
|
||
SSG_ROIRectD _getListROI(std::vector< SVzNL3DPosition>& listData)
|
||
{
|
||
if (listData.size() == 0)
|
||
return { 0,0,0,0 };
|
||
SSG_ROIRectD roi = { listData[0].pt3D.x, listData[0].pt3D.x, listData[0].pt3D.y, listData[0].pt3D.y };
|
||
for (int i = 0; i < (int)listData.size(); i++)
|
||
{
|
||
roi.left = roi.left > listData[i].pt3D.x ? listData[i].pt3D.x : roi.left;
|
||
roi.right = roi.right < listData[i].pt3D.x ? listData[i].pt3D.x : roi.right;
|
||
roi.top = roi.top > listData[i].pt3D.y ? listData[i].pt3D.y : roi.top;
|
||
roi.bottom = roi.bottom < listData[i].pt3D.y ? listData[i].pt3D.y : roi.bottom;
|
||
}
|
||
return roi;
|
||
}
|
||
|
||
double _getListMeanZ(std::vector< SVzNL3DPosition>& listData, SVzNLRangeD& zRange)
|
||
{
|
||
if (listData.size() == 0)
|
||
return 0;
|
||
double meanZ = 0;
|
||
zRange.max = -1;
|
||
zRange.min = 0;
|
||
for (int i = 0; i < (int)listData.size(); i++)
|
||
{
|
||
meanZ += listData[i].pt3D.z;
|
||
if (zRange.max < 0)
|
||
{
|
||
zRange.max = listData[i].pt3D.z;
|
||
zRange.min = listData[i].pt3D.z;
|
||
}
|
||
else
|
||
{
|
||
zRange.max = zRange.max < listData[i].pt3D.z ? listData[i].pt3D.z : zRange.max;
|
||
zRange.min = zRange.min > listData[i].pt3D.z ? listData[i].pt3D.z : zRange.min;
|
||
}
|
||
}
|
||
meanZ = meanZ / (double)listData.size();
|
||
return meanZ;
|
||
}
|
||
|
||
//逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0
|
||
cv::Point2f _rotate2D(cv::Point2f pt, double sinTheta, double cosTheta)
|
||
{
|
||
return (cv::Point2f((float)(pt.x * cosTheta - pt.y * sinTheta), (float)(pt.x * sinTheta + pt.y * cosTheta)));
|
||
}
|
||
|
||
//料筐码放：获取料筐尺寸、料筐姿态、料筐中心点坐标
|
||
#if 0
|
||
//检测层板边缘。层板小于料筐
|
||
WD_HRM_BinInfo wd_HRM_getBinSize(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const SSG_cornerParam cornerPara,
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
WD_HRM_BinInfo resultPose;
|
||
memset(&resultPose, 0, sizeof(WD_HRM_BinInfo));
|
||
|
||
int lineNum = (int)scanLines.size();
|
||
if (lineNum == 0)
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_NULL;
|
||
return resultPose;
|
||
}
|
||
|
||
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 resultPose;
|
||
}
|
||
|
||
//产生水平扫描数据
|
||
std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
|
||
scanLines_h.resize(linePtNum);
|
||
for (int i = 0; i < linePtNum; i++)
|
||
scanLines_h[i].resize(lineNum);
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
scanLines_h[j][line] = scanLines[line][j];
|
||
scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
||
scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
||
}
|
||
}
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++)
|
||
scanLines_h[line][j].nPointIdx = j;
|
||
}
|
||
|
||
//算法流程：
|
||
//1、检查垂直方向数据并去除
|
||
//2、聚类
|
||
//3、保留最大目标
|
||
//4、拟合
|
||
|
||
//内部参数
|
||
SSG_cornerParam removeVertialPara = cornerPara;
|
||
removeVertialPara.scale = 3.0;
|
||
removeVertialPara.cornerTh = 60;
|
||
|
||
std::vector<std::vector<int>> flags;
|
||
flags.resize(lineNum);
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
flags[i].resize(linePtNum);
|
||
std::fill(flags[i].begin(), flags[i].end(), 0);
|
||
}
|
||
std::vector<std::vector<int>> zVertivalFlags;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 700)
|
||
int kkk = 1;
|
||
std::vector<int> line_verticalFlags;
|
||
wd_getXYVertialFeature_dirAngleMethod(
|
||
scanLines[line],
|
||
line,
|
||
removeVertialPara,
|
||
line_verticalFlags
|
||
);
|
||
zVertivalFlags.push_back(line_verticalFlags);
|
||
|
||
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
||
{
|
||
if (line_verticalFlags[i] > 0)
|
||
flags[line][i] = 1;
|
||
}
|
||
}
|
||
|
||
std::vector<std::vector<int>> zVertivalFlags_h;
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
if (line == 1177)
|
||
int kkk = 1;
|
||
std::vector<int> line_verticalFlags;
|
||
wd_getXYVertialFeature_dirAngleMethod(
|
||
scanLines_h[line],
|
||
line,
|
||
removeVertialPara,
|
||
line_verticalFlags
|
||
);
|
||
zVertivalFlags_h.push_back(line_verticalFlags);
|
||
|
||
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
||
{
|
||
if (line_verticalFlags[i] > 0)
|
||
flags[i][line] = 1;
|
||
}
|
||
}
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if (flags[line][j] > 0)
|
||
{
|
||
scanLines[line][j].pt3D.z = 0;
|
||
scanLines_h[j][line].pt3D.z = 0;
|
||
}
|
||
}
|
||
}
|
||
//迭代一次
|
||
SSG_lineSegParam lineSegPara;
|
||
lineSegPara.distScale = 5.0;
|
||
lineSegPara.segGapTh_y = 5.0;
|
||
lineSegPara.segGapTh_z = 5.0;
|
||
const int minSegLen = 5;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
std::vector<SSG_RUN> segs;
|
||
wd_getLineDataIntervals(
|
||
scanLines[line],
|
||
lineSegPara,
|
||
segs);
|
||
for (int i = 0; i < (int)segs.size(); i++)
|
||
{
|
||
if (segs[i].len <= minSegLen)
|
||
{
|
||
int idx0 = segs[i].start;
|
||
for (int j = 0; j < segs[i].len; j++)
|
||
flags[line][idx0 + j] = 1;
|
||
}
|
||
}
|
||
}
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
std::vector<SSG_RUN> segs;
|
||
wd_getLineDataIntervals(
|
||
scanLines_h[line],
|
||
lineSegPara,
|
||
segs);
|
||
for (int i = 0; i < (int)segs.size(); i++)
|
||
{
|
||
if (segs[i].len <= minSegLen)
|
||
{
|
||
int idx0 = segs[i].start;
|
||
for (int j = 0; j < segs[i].len; j++)
|
||
flags[idx0 + j][line] = 1;
|
||
}
|
||
}
|
||
}
|
||
|
||
//标注
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
}
|
||
//将垂直线段去除
|
||
std::vector< SVzNL3DPosition> validPoints;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if (flags[line][j] > 0)
|
||
scanLines[line][j].pt3D.z = 0;
|
||
|
||
if (scanLines[line][j].pt3D.z > 1e-4)
|
||
{
|
||
SVzNL3DPosition a_vldPt;
|
||
a_vldPt.pt3D = scanLines[line][j].pt3D;
|
||
a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
|
||
validPoints.push_back(a_vldPt);
|
||
}
|
||
}
|
||
}
|
||
|
||
//聚类
|
||
//内部参数
|
||
//double minObjSize_w = 150;
|
||
//double minObjSize_h = 150;
|
||
|
||
int clusterCheckWin = 5;
|
||
double clusterDist = 5.0;
|
||
int distType = 1; //0 - 2d distance; 1- 3d distance
|
||
std::vector<std::vector< SVzNL3DPosition>> objClusters; //result
|
||
wd_pointClustering_speedUp(
|
||
validPoints,
|
||
lineNum, linePtNum, clusterCheckWin, //搜索窗口
|
||
clusterDist,
|
||
distType,
|
||
objClusters //result
|
||
);
|
||
|
||
//保留最大的目标
|
||
std::vector<double> objMeanZ;
|
||
std::vector<SVzNLRangeD> objZRange;
|
||
objMeanZ.resize(objClusters.size());
|
||
objZRange.resize(objClusters.size());
|
||
int maxSizeId = -1;
|
||
double maxSize = 0;
|
||
for (int i = 0; i < (int)objClusters.size(); i++)
|
||
{
|
||
SSG_ROIRectD a_roi = _getListROI(objClusters[i]);
|
||
double w = a_roi.right - a_roi.left;
|
||
double h = a_roi.bottom - a_roi.top;
|
||
double size = w * h;
|
||
|
||
SVzNLRangeD zRange;
|
||
double meanZ = _getListMeanZ(objClusters[i], zRange);
|
||
objMeanZ[i] = meanZ;
|
||
objZRange[i] = zRange;
|
||
|
||
if (maxSize < size)
|
||
{
|
||
maxSize = size;
|
||
maxSizeId = i;
|
||
}
|
||
|
||
}
|
||
|
||
//迭代：消除底板和层板的边缘的扫描毛刺
|
||
|
||
|
||
|
||
std::vector< SVzNL3DPosition>& bottomCluster = objClusters[maxSizeId];
|
||
//标注
|
||
//重新将flags设置为目标的mask
|
||
for (int i = 0; i < lineNum; i++)
|
||
std::fill(flags[i].begin(), flags[i].end(), -1);
|
||
for (int i = 0; i < (int)bottomCluster.size(); i++)
|
||
{
|
||
int line = bottomCluster[i].nPointIdx >> 16;
|
||
int ptIdx = bottomCluster[i].nPointIdx & 0x0000FFFF;
|
||
scanLines[line][ptIdx].nPointIdx = 2;
|
||
flags[line][ptIdx] = i; //indexing
|
||
}
|
||
|
||
//使用PCA方法计算法向量
|
||
SVzNL3DPoint vec_normal, vec_centroid;
|
||
|
||
computePlaneNormalByPCA(
|
||
bottomCluster,
|
||
vec_normal,
|
||
vec_centroid);
|
||
|
||
//投影
|
||
|
||
|
||
|
||
|
||
|
||
if (vec_normal.z < 0)
|
||
vec_normal = { -vec_normal.x, -vec_normal.y, -vec_normal.z };
|
||
resultPose.center = vec_centroid;
|
||
resultPose.bottomNormal = vec_normal;
|
||
|
||
|
||
return resultPose;
|
||
}
|
||
#else
|
||
//检测料筐边缘，以中心点为基准进行规划
|
||
WD_HRM_BinInfo wd_HRM_getBinSize(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const SSG_planeCalibPara calibPara,
|
||
const double binHeight, //料筐高度
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
WD_HRM_BinInfo resultPose;
|
||
memset(&resultPose, 0, sizeof(WD_HRM_BinInfo));
|
||
|
||
//内部参数
|
||
SVzNLRangeD binTopSliceRange = {calibPara.planeHeight- binHeight-5.0, calibPara.planeHeight - binHeight + 5.0}; //料筐高度Z层切范围
|
||
|
||
int lineNum = (int)scanLines.size();
|
||
if (lineNum == 0)
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_NULL;
|
||
return resultPose;
|
||
}
|
||
|
||
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 resultPose;
|
||
}
|
||
|
||
//地面调平
|
||
for (int i = 0; i < lineNum; i++)
|
||
wd_lineDataR(scanLines[i], calibPara.planeCalib, -1);//调平
|
||
|
||
//Z层切
|
||
std::vector<SVzNL3DPosition> zSliceData;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
scanLines[line][j].nPointIdx = 0;
|
||
if ((scanLines[line][j].pt3D.z > binTopSliceRange.min) && (scanLines[line][j].pt3D.z < binTopSliceRange.max))
|
||
{
|
||
SVzNL3DPosition a_pt;
|
||
a_pt.nPointIdx = (line << 16) | j & 0xffff;
|
||
a_pt.pt3D = scanLines[line][j].pt3D;
|
||
zSliceData.push_back(a_pt);
|
||
scanLines[line][j].nPointIdx = 1; //标注
|
||
}
|
||
}
|
||
}
|
||
|
||
SVzNLRangeD dataZRange;
|
||
double zSliceZ = _getListMeanZ(zSliceData, dataZRange);
|
||
//计算轮廓
|
||
// 最小外接矩形
|
||
std::vector<cv::Point2f> points;
|
||
for (int i = 0; i < (int)zSliceData.size(); i++)
|
||
{
|
||
cv::Point2f a_pt = cv::Point2f(zSliceData[i].pt3D.x, zSliceData[i].pt3D.y);
|
||
points.push_back(a_pt);
|
||
}
|
||
cv::RotatedRect rect = minAreaRect(points);
|
||
cv::Point2f vertices[4];
|
||
rect.points(vertices);
|
||
double width = rect.size.width; //投影的宽和高
|
||
double height = rect.size.height;
|
||
if (width < height)
|
||
{
|
||
double tmp = height;
|
||
height = width;
|
||
width = tmp;
|
||
}
|
||
//计算姿态角vertices[0]是旋转点
|
||
double dist_v0v3 = sqrt(pow(vertices[0].x - vertices[3].x, 2) + pow(vertices[0].y - vertices[3].y, 2));
|
||
double width_diff = abs(dist_v0v3 - width);
|
||
double pose_yaw;
|
||
if (CV_VERSION == "3.2.0")
|
||
{
|
||
if (width_diff < 10.0)//width方向
|
||
{
|
||
pose_yaw = -rect.angle;
|
||
}
|
||
else //长度方向
|
||
{
|
||
pose_yaw = -rect.angle - 90;
|
||
if (pose_yaw < -90)
|
||
pose_yaw = 180 + pose_yaw;
|
||
}
|
||
}
|
||
else //if (CV_VERSION == "4.8.0")
|
||
{
|
||
if (width_diff < 10.0) //width方向
|
||
{
|
||
pose_yaw = -rect.angle;
|
||
}
|
||
else//长度方向
|
||
{
|
||
pose_yaw = -rect.angle + 90;
|
||
if (pose_yaw > 90)
|
||
pose_yaw = pose_yaw - 180;
|
||
}
|
||
}
|
||
//生成料筐信息
|
||
double binZ = calibPara.planeHeight - binHeight;
|
||
double sinTheta = sin(-PI * pose_yaw / 180);
|
||
double cosTheta = cos(-PI * pose_yaw / 180);
|
||
resultPose.center = { rect.center.x, rect.center.y, binZ };
|
||
resultPose.bottomNormal = { 0.0, 0.0, 1.0 };
|
||
resultPose.x_dir = { cosTheta , sinTheta , 0.0 };
|
||
resultPose.y_dir = vec3_cross(resultPose.bottomNormal, resultPose.x_dir); //叉乘出y_dir
|
||
resultPose.length = width;
|
||
resultPose.width = height;
|
||
resultPose.binTopZ = zSliceZ;
|
||
resultPose.minRectVertex[0] = { vertices[0].x, vertices[0].y, binZ };
|
||
resultPose.minRectVertex[1] = { vertices[1].x, vertices[1].y, binZ };
|
||
resultPose.minRectVertex[2] = { vertices[2].x, vertices[2].y, binZ };
|
||
resultPose.minRectVertex[3] = { vertices[3].x, vertices[3].y, binZ };
|
||
|
||
return resultPose;
|
||
}
|
||
#endif
|
||
|
||
//料筐码放：获取工件尺寸
|
||
WD_HRM_workpieceSizeInfo wd_HRM_getWorkpieceSize(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
std::vector<WD_HRM_workpieceSizeInfo>& standardWorkpieceSize,
|
||
const SSG_cornerParam cornerPara,
|
||
const SSG_planeCalibPara calibPara,
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
WD_HRM_workpieceSizeInfo resultInfo;
|
||
memset(&resultInfo, 0, sizeof(WD_HRM_workpieceSizeInfo));
|
||
|
||
int lineNum = (int)scanLines.size();
|
||
if (lineNum == 0)
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_NULL;
|
||
return resultInfo;
|
||
}
|
||
|
||
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 resultInfo;
|
||
}
|
||
|
||
//地面调平
|
||
for (int i = 0; i < lineNum; i++)
|
||
wd_lineDataR(scanLines[i], calibPara.planeCalib, -1);//调平
|
||
|
||
//产生水平扫描数据
|
||
std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
|
||
scanLines_h.resize(linePtNum);
|
||
for (int i = 0; i < linePtNum; i++)
|
||
scanLines_h[i].resize(lineNum);
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
scanLines_h[j][line] = scanLines[line][j];
|
||
scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
||
scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
||
}
|
||
}
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++)
|
||
scanLines_h[line][j].nPointIdx = j;
|
||
}
|
||
|
||
//算法流程：
|
||
//1、检查垂直方向数据并去除
|
||
//2、聚类
|
||
//3、保留最大目标
|
||
//4、拟合
|
||
|
||
//内部参数
|
||
SSG_cornerParam removeVertialPara = cornerPara;
|
||
removeVertialPara.scale = 3.0;
|
||
removeVertialPara.cornerTh = 60;
|
||
|
||
std::vector<std::vector<int>> flags;
|
||
flags.resize(lineNum);
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
flags[i].resize(linePtNum);
|
||
std::fill(flags[i].begin(), flags[i].end(), 0);
|
||
}
|
||
std::vector<std::vector<int>> zVertivalFlags;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 700)
|
||
int kkk = 1;
|
||
std::vector<int> line_verticalFlags;
|
||
wd_getXYVertialFeature_dirAngleMethod(
|
||
scanLines[line],
|
||
line,
|
||
removeVertialPara,
|
||
line_verticalFlags
|
||
);
|
||
zVertivalFlags.push_back(line_verticalFlags);
|
||
|
||
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
||
{
|
||
if (line_verticalFlags[i] > 0)
|
||
flags[line][i] = 1;
|
||
}
|
||
}
|
||
|
||
std::vector<std::vector<int>> zVertivalFlags_h;
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
if (line == 1177)
|
||
int kkk = 1;
|
||
std::vector<int> line_verticalFlags;
|
||
wd_getXYVertialFeature_dirAngleMethod(
|
||
scanLines_h[line],
|
||
line,
|
||
removeVertialPara,
|
||
line_verticalFlags
|
||
);
|
||
zVertivalFlags_h.push_back(line_verticalFlags);
|
||
|
||
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
||
{
|
||
if (line_verticalFlags[i] > 0)
|
||
flags[i][line] = 1;
|
||
}
|
||
}
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if (flags[line][j] > 0)
|
||
{
|
||
scanLines[line][j].pt3D.z = 0;
|
||
scanLines_h[j][line].pt3D.z = 0;
|
||
}
|
||
}
|
||
}
|
||
//迭代一次
|
||
SSG_lineSegParam lineSegPara;
|
||
lineSegPara.distScale = 5.0;
|
||
lineSegPara.segGapTh_y = 5.0;
|
||
lineSegPara.segGapTh_z = 5.0;
|
||
const int minSegLen = 5;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
std::vector<SSG_RUN> segs;
|
||
wd_getLineDataIntervals(
|
||
scanLines[line],
|
||
lineSegPara,
|
||
segs);
|
||
for (int i = 0; i < (int)segs.size(); i++)
|
||
{
|
||
if (segs[i].len <= minSegLen)
|
||
{
|
||
int idx0 = segs[i].start;
|
||
for (int j = 0; j < segs[i].len; j++)
|
||
flags[line][idx0 + j] = 1;
|
||
}
|
||
}
|
||
}
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
std::vector<SSG_RUN> segs;
|
||
wd_getLineDataIntervals(
|
||
scanLines_h[line],
|
||
lineSegPara,
|
||
segs);
|
||
for (int i = 0; i < (int)segs.size(); i++)
|
||
{
|
||
if (segs[i].len <= minSegLen)
|
||
{
|
||
int idx0 = segs[i].start;
|
||
for (int j = 0; j < segs[i].len; j++)
|
||
flags[idx0 + j][line] = 1;
|
||
}
|
||
}
|
||
}
|
||
|
||
//标注
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
}
|
||
//将垂直线段去除
|
||
std::vector< SVzNL3DPosition> validPoints;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if (flags[line][j] > 0)
|
||
scanLines[line][j].pt3D.z = 0;
|
||
|
||
if (scanLines[line][j].pt3D.z > 1e-4)
|
||
{
|
||
SVzNL3DPosition a_vldPt;
|
||
a_vldPt.pt3D = scanLines[line][j].pt3D;
|
||
a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
|
||
validPoints.push_back(a_vldPt);
|
||
}
|
||
}
|
||
}
|
||
|
||
//聚类
|
||
//内部参数
|
||
//double minObjSize_w = 150;
|
||
//double minObjSize_h = 150;
|
||
|
||
int clusterCheckWin = 5;
|
||
double clusterDist = 10.0;
|
||
int distType = 1; //0 - 2d distance; 1- 3d distance
|
||
std::vector<std::vector< SVzNL3DPosition>> objClusters; //result
|
||
wd_pointClustering_speedUp(
|
||
validPoints,
|
||
lineNum, linePtNum, clusterCheckWin, //搜索窗口
|
||
clusterDist,
|
||
distType,
|
||
objClusters //result
|
||
);
|
||
|
||
//取最大的目标为层板
|
||
std::vector<double> objMeanZ;
|
||
std::vector<SVzNLRangeD> objZRange;
|
||
std::vector< SSG_ROIRectD> objROIs;
|
||
objMeanZ.resize(objClusters.size());
|
||
objZRange.resize(objClusters.size());
|
||
objROIs.resize(objClusters.size());
|
||
int maxSizeId = -1;
|
||
double maxSize = 0;
|
||
for (int i = 0; i < (int)objClusters.size(); i++)
|
||
{
|
||
SSG_ROIRectD a_roi = _getListROI(objClusters[i]);
|
||
objROIs[i] = a_roi;
|
||
|
||
SVzNLRangeD zRange;
|
||
double meanZ = _getListMeanZ(objClusters[i], zRange);
|
||
objMeanZ[i] = meanZ;
|
||
objZRange[i] = zRange;
|
||
|
||
double w = a_roi.right - a_roi.left;
|
||
double h = a_roi.bottom - a_roi.top;
|
||
double size = w * h;
|
||
if (maxSize < size)
|
||
{
|
||
maxSize = size;
|
||
maxSizeId = i;
|
||
}
|
||
}
|
||
|
||
//取层板ROI内的目标为工件
|
||
SSG_ROIRectD& layerBoardROI = objROIs[maxSizeId];
|
||
double layerBoardZ = objMeanZ[maxSizeId];
|
||
int workpieceClusterId = -1;
|
||
for (int i = 0; i< (int)objClusters.size(); i++)
|
||
{
|
||
if (i == maxSizeId)
|
||
continue;
|
||
|
||
SSG_ROIRectD& a_roi = objROIs[i];
|
||
double obj_z = objMeanZ[i];
|
||
if ((a_roi.left > layerBoardROI.left) && (a_roi.right < layerBoardROI.right) &&
|
||
(a_roi.top > layerBoardROI.top) && (a_roi.bottom < layerBoardROI.bottom) && (obj_z < layerBoardZ))
|
||
{
|
||
if (workpieceClusterId < 0)
|
||
workpieceClusterId = i;
|
||
else if (objClusters[workpieceClusterId].size() < objClusters[i].size())
|
||
workpieceClusterId = i;
|
||
}
|
||
}
|
||
|
||
if(workpieceClusterId <0)
|
||
{
|
||
*errCode = SX_ERR_ZERO_OBJECTS;
|
||
return resultInfo;
|
||
}
|
||
|
||
std::vector< SVzNL3DPosition>& layerBoardCluster = objClusters[maxSizeId];
|
||
std::vector< SVzNL3DPosition>& workpieceCluster = objClusters[workpieceClusterId];
|
||
double workpieceHeight = layerBoardZ - objZRange[workpieceClusterId].min;
|
||
//标注
|
||
for (int i = 0; i < (int)layerBoardCluster.size(); i++)
|
||
{
|
||
int line = layerBoardCluster[i].nPointIdx >> 16;
|
||
int ptIdx = layerBoardCluster[i].nPointIdx & 0x0000FFFF;
|
||
scanLines[line][ptIdx].nPointIdx = 1;
|
||
}
|
||
for (int i = 0; i < (int)workpieceCluster.size(); i++)
|
||
{
|
||
int line = workpieceCluster[i].nPointIdx >> 16;
|
||
int ptIdx = workpieceCluster[i].nPointIdx & 0x0000FFFF;
|
||
scanLines[line][ptIdx].nPointIdx = 2;
|
||
}
|
||
|
||
//计算工件的最小外接圆
|
||
std::vector<cv::Point2f> points_2d;
|
||
for (int i = 0; i < (int)workpieceCluster.size(); i++)
|
||
{
|
||
cv::Point2f a_pt = cv::Point2f(workpieceCluster[i].pt3D.x, workpieceCluster[i].pt3D.y);
|
||
points_2d.push_back(a_pt);
|
||
}
|
||
cv::Point2f center;
|
||
float r;
|
||
cv::minEnclosingCircle(points_2d, center, r);
|
||
|
||
//关联标准工件
|
||
double bestError = 0;
|
||
int bestId = -1;
|
||
for (int i = 0; i < (int)standardWorkpieceSize.size(); i++)
|
||
{
|
||
double err = abs(standardWorkpieceSize[i].workpieceHeight - workpieceHeight) + abs(standardWorkpieceSize[i].workpieceRadius - r);
|
||
if (bestId < 0)
|
||
{
|
||
bestId = i;
|
||
bestError = err;
|
||
}
|
||
else if (bestError > err)
|
||
{
|
||
bestId = i;
|
||
bestError = err;
|
||
}
|
||
}
|
||
//生成投影结果
|
||
resultInfo.center = { center.x, center.y, layerBoardZ };
|
||
resultInfo.layerZValue = layerBoardZ;
|
||
resultInfo.workpieceHeight = standardWorkpieceSize[bestId].workpieceHeight;
|
||
resultInfo.workpieceRadius = standardWorkpieceSize[bestId].workpieceRadius;
|
||
return resultInfo;
|
||
}
|
||
|
||
void planningFromCenter_1D(int num, double interval, double centerValue, std::vector<double>& pos)
|
||
{
|
||
pos.resize(num);
|
||
double halfInterval = interval / 2;
|
||
if (num % 2 == 1) //奇数
|
||
{
|
||
int centerIdx = num / 2;
|
||
pos[centerIdx] = centerValue;
|
||
int j = 1;
|
||
for (int idx = centerIdx - 1; idx >= 0; idx--)
|
||
{
|
||
pos[idx] = centerValue - j * interval;
|
||
j++;
|
||
}
|
||
j = 1;
|
||
for (int idx = centerIdx + 1; idx < num; idx++)
|
||
{
|
||
pos[idx] = centerValue + j * interval;
|
||
j++;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
int j = 0;
|
||
int halfSize = num / 2;
|
||
for (int idx = halfSize - 1; idx >= 0; idx--)
|
||
{
|
||
pos[idx] = centerValue - halfInterval - j * interval;
|
||
j++;
|
||
}
|
||
j = 0;
|
||
for (int idx = halfSize; idx < num; idx++)
|
||
{
|
||
pos[idx] = centerValue + halfInterval + j * interval;
|
||
j++;
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
//料筐码放：码放位置规划
|
||
void wd_HRM_PlanBinPlacement(
|
||
const WD_HRM_BinInfo binInfo,
|
||
const SSG_size2D realBoardSize, //实际的托板大小
|
||
const WD_HRM_workpieceSizeInfo workpieceInfo,
|
||
const SSG_planeCalibPara calibPara,
|
||
const double guardingInterval, //工作与工件的保护间隔
|
||
std::vector< WD_workpieceInfo>& planningPositions,
|
||
int* out_rows,
|
||
int* out_cols,
|
||
int* isLastLayere)
|
||
{
|
||
double diamter = workpieceInfo.workpieceRadius * 2 + guardingInterval;
|
||
double L = realBoardSize.width;// binInfo.length - guardingToSide * 2 + guardingInterval;
|
||
double W = realBoardSize.height; //binInfo.width - guardingToSide * 2 + guardingInterval;
|
||
|
||
int cols = (int)(L / diamter);
|
||
int rows = (int)(W / diamter);
|
||
|
||
//以中心为基准进行位置计算
|
||
std::vector<double> rowPos;
|
||
planningFromCenter_1D(rows, diamter, 0, rowPos);
|
||
std::vector<double> colPos;
|
||
planningFromCenter_1D(cols, diamter, 0, colPos);
|
||
|
||
int objNum = rows * cols;
|
||
planningPositions.resize(objNum);
|
||
for (int row = 0; row < rows; row++)
|
||
{
|
||
for(int col = 0; col < cols; col++)
|
||
{
|
||
WD_workpieceInfo a_pos;
|
||
a_pos.center = { colPos[col], rowPos[row], workpieceInfo.layerZValue };
|
||
a_pos.value = workpieceInfo.workpieceRadius;
|
||
a_pos.workpieceType = 1;
|
||
a_pos.z_dir = { 0, 0, 1.0 };
|
||
a_pos.y_dir = { 0, 0, 0 };
|
||
a_pos.x_dir = { 0, 0, 0 };
|
||
planningPositions[row * cols + col] = a_pos;
|
||
}
|
||
}
|
||
|
||
//旋转
|
||
double cosTheta = binInfo.x_dir.x;
|
||
double sinTheta = binInfo.x_dir.y;
|
||
for (int i = 0; i < (int)planningPositions.size(); i++)
|
||
{
|
||
cv::Point2f a_pt2D = cv::Point2f(planningPositions[i].center.x, planningPositions[i].center.y);
|
||
a_pt2D = _rotate2D(a_pt2D, sinTheta, cosTheta);
|
||
planningPositions[i].center.x = a_pt2D.x + binInfo.center.x;
|
||
planningPositions[i].center.y = a_pt2D.y + binInfo.center.y;
|
||
}
|
||
|
||
double resiH = workpieceInfo.layerZValue - binInfo.binTopZ - workpieceInfo.workpieceHeight;
|
||
if (resiH < workpieceInfo.workpieceHeight)
|
||
*isLastLayere = 1;
|
||
else
|
||
*isLastLayere = 0;
|
||
*out_rows = rows;
|
||
*out_cols = cols;
|
||
|
||
//旋转回原坐标系
|
||
for (int i = 0; i < (int)planningPositions.size(); i++)
|
||
{
|
||
planningPositions[i].center = wd_ptRotate(planningPositions[i].center, calibPara.invRMatrix);
|
||
planningPositions[i].x_dir = wd_ptRotate(planningPositions[i].x_dir, calibPara.invRMatrix);
|
||
planningPositions[i].y_dir = wd_ptRotate(planningPositions[i].y_dir, calibPara.invRMatrix);
|
||
planningPositions[i].z_dir = wd_ptRotate(planningPositions[i].z_dir, calibPara.invRMatrix);
|
||
}
|
||
|
||
return;
|
||
} |