1289 lines
33 KiB
C++
1289 lines
33 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 : 海瑞马码垛规划版本
|
||
std::string m_strVersion = "HybridPositioning 1.1.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;
|
||
}
|
||
|
||
WD_workpieceInfo _computeWorkpiecePose(std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||
{
|
||
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;
|
||
}
|
||
}
|
||
//垂直和水平扫描，计算水平和垂直方向角度
|
||
|
||
|
||
|
||
void wd_computeDirAngle_wholeLine2(
|
||
std::vector< SVzNL3DPosition>&line_data,
|
||
const double steppingScale,
|
||
const double invalidScale, //超出此尺度，方向角计算无效
|
||
std::vector< SSG_pntDirAngle>&ptDirAngles
|
||
);
|
||
|
||
//检测是否有凸起
|
||
return a_pose;
|
||
}
|
||
|
||
|
||
void wd_HRM_RotorCorePositioning(
|
||
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;
|
||
}
|
||
|
||
#if 0
|
||
for (int i = 0; i < lineNum; i++)
|
||
{ //行处理
|
||
//调平，去除地面
|
||
wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
|
||
}
|
||
#endif
|
||
|
||
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;
|
||
const int imgRows = maxV;
|
||
std::vector<std::vector<SWDIndexingVzPoint>> mappingTable; //按图像大小建立。
|
||
mappingTable.resize(imgCols); //与扫描线方向对应
|
||
for (int i = 0; i < imgCols; i++)
|
||
mappingTable[i].resize(imgRows);
|
||
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
|
||
{
|
||
scanLinesInput[line][ptIdx].nPointIdx = 0;
|
||
if (scanLinesInput[line][ptIdx].pt3D.z < 1e-4)
|
||
continue;
|
||
|
||
SWDIndexingVzPoint indexingPt;
|
||
indexingPt.lineIdx = line;
|
||
indexingPt.ptIdx = ptIdx;
|
||
indexingPt.point = scanLinesInput[line][ptIdx].pt3D;
|
||
int u = scanLinesInput[line][ptIdx].ptLeft2D.x;
|
||
int v = scanLinesInput[line][ptIdx].ptLeft2D.y;
|
||
mappingTable[u][v] = indexingPt;
|
||
}
|
||
}
|
||
|
||
int objNum = (int)objROIs.size();
|
||
for (int idx = 0; idx < objNum; idx++)
|
||
{
|
||
WD_objArea2D& obj_roi = objROIs[idx];
|
||
int L = (int)(obj_roi.roi.left + 0.5);
|
||
int R = (int)(obj_roi.roi.right + 0.5);
|
||
int T = (int)(obj_roi.roi.top + 0.5);
|
||
int B = (int)(obj_roi.roi.bottom + 0.5);
|
||
|
||
//统计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++)
|
||
{
|
||
if (mappingTable[x][y].point.z > 1e-4)
|
||
{
|
||
int lineIdx = mappingTable[x][y].lineIdx;
|
||
int ptIdx = mappingTable[x][y].ptIdx;
|
||
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中的扫描数据
|
||
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++)
|
||
{
|
||
if (mappingTable[x][y].point.z > 1e-4)
|
||
{
|
||
int lineIdx = mappingTable[x][y].lineIdx - roiLineIndice.nMin;
|
||
int ptIdx = mappingTable[x][y].ptIdx - roiPtIndice.nMin;
|
||
roiScanLines[lineIdx][ptIdx].pt3D = mappingTable[x][y].point;
|
||
}
|
||
}
|
||
}
|
||
|
||
//判断工件种类，计算工件姿态
|
||
|
||
|
||
}
|
||
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++)
|
||
{
|
||
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;
|
||
} |