hybridPosePositioning
version 1.3.9 : 转子钢芯， 改进了料筐检测方法
This commit is contained in:
parent
737d741406
commit
bf8b449e48
@ -674,6 +674,7 @@ void _outputRGBDResult_XYZ_RGBD_workpieceInfo(
|
||||
char* fileName,
|
||||
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
|
||||
std::vector<WD_workpieceInfo>& poseInfo,
|
||||
WD_HRM_BinInfo* binInfo,
|
||||
double dirLen)
|
||||
{
|
||||
int objNumber = 0;
|
||||
@ -793,6 +794,55 @@ void _outputRGBDResult_XYZ_RGBD_workpieceInfo(
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
lineIdx++;
|
||||
}
|
||||
|
||||
if (binInfo)
|
||||
{
|
||||
rgb = { 0, 0, 250 };
|
||||
//输出外接矩形
|
||||
SVzNL3DPoint dirPt1 = binInfo->minRectVertex[0];
|
||||
SVzNL3DPoint dirPt2 = binInfo->minRectVertex[1];
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << dirPt1.x << "," << dirPt1.y << "," << dirPt1.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << dirPt2.x << "," << dirPt2.y << "," << dirPt2.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
lineIdx++;
|
||||
|
||||
dirPt1 = binInfo->minRectVertex[1];
|
||||
dirPt2 = binInfo->minRectVertex[2];
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << dirPt1.x << "," << dirPt1.y << "," << dirPt1.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << dirPt2.x << "," << dirPt2.y << "," << dirPt2.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
lineIdx++;
|
||||
|
||||
dirPt1 = binInfo->minRectVertex[2];
|
||||
dirPt2 = binInfo->minRectVertex[3];
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << dirPt1.x << "," << dirPt1.y << "," << dirPt1.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << dirPt2.x << "," << dirPt2.y << "," << dirPt2.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
lineIdx++;
|
||||
|
||||
dirPt1 = binInfo->minRectVertex[3];
|
||||
dirPt2 = binInfo->minRectVertex[0];
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << dirPt1.x << "," << dirPt1.y << "," << dirPt1.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << dirPt2.x << "," << dirPt2.y << "," << dirPt2.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
lineIdx++;
|
||||
}
|
||||
}
|
||||
|
||||
sw.close();
|
||||
@ -1651,6 +1701,7 @@ void _XOYprojection_workpieceInfo(
|
||||
cv::Mat& img,
|
||||
std::vector<std::vector< SVzNL3DPosition>>& dataLines,
|
||||
std::vector<WD_workpieceInfo>& workpieceInfo,
|
||||
std::vector<SVzNL3DPoint>& binRectVertex,
|
||||
const double scale,
|
||||
const int sideWidth,
|
||||
const SVzNLRangeD x_range,
|
||||
@ -1719,6 +1770,24 @@ void _XOYprojection_workpieceInfo(
|
||||
cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), 2);
|
||||
}
|
||||
}
|
||||
if (binRectVertex.size() == 4)
|
||||
{
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
int idx_0 = i;
|
||||
int idx_1 = (i + 1) % 4;
|
||||
double x1 = binRectVertex[idx_0].x;
|
||||
double y1 = binRectVertex[idx_0].y;
|
||||
int px1 = (int)((x1 - x_range.min) / scale + sideWidth);
|
||||
int py1 = (int)((y1 - y_range.min) / scale + sideWidth);
|
||||
double x2 = binRectVertex[idx_1].x;
|
||||
double y2 = binRectVertex[idx_1].y;
|
||||
int px2 = (int)((x2 - x_range.min) / scale + sideWidth);
|
||||
int py2 = (int)((y2 - y_range.min) / scale + sideWidth);
|
||||
rgb = { 0, 0, 255 }; //y轴用绿色
|
||||
cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void _genXOYProjectionImage_workpieceInfo(
|
||||
@ -1726,6 +1795,7 @@ void _genXOYProjectionImage_workpieceInfo(
|
||||
std::vector<std::vector< SVzNL3DPosition>>& scanLines,
|
||||
const double scale,
|
||||
std::vector<WD_workpieceInfo>& workpieceInfo,
|
||||
WD_HRM_BinInfo* binInfo,
|
||||
const double rpy[3], const double dirLen)
|
||||
{
|
||||
//旋转视角显示
|
||||
@ -1749,7 +1819,40 @@ void _genXOYProjectionImage_workpieceInfo(
|
||||
SVzNLRangeD rx_range, ry_range;
|
||||
_rotateCloudPts(scanLines, matrix3d, rotateLines, rx_range, ry_range);
|
||||
|
||||
//统计X和Y的范围
|
||||
std::vector<SVzNL3DPoint> binRectVertex;//最小外接矩形顶点
|
||||
if (binInfo)
|
||||
{
|
||||
binRectVertex.resize(4);
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
binRectVertex[i] = _ptRotate(binInfo->minRectVertex[i], matrix3d);
|
||||
if (rx_range.max < rx_range.min)
|
||||
{
|
||||
rx_range.min = binRectVertex[i].x;
|
||||
rx_range.max = binRectVertex[i].x;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (rx_range.min > binRectVertex[i].x)
|
||||
rx_range.min = binRectVertex[i].x;
|
||||
if (rx_range.max < binRectVertex[i].x)
|
||||
rx_range.max = binRectVertex[i].x;
|
||||
}
|
||||
if (ry_range.max < ry_range.min)
|
||||
{
|
||||
ry_range.min = binRectVertex[i].y;
|
||||
ry_range.max = binRectVertex[i].y;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (ry_range.min > binRectVertex[i].y)
|
||||
ry_range.min = binRectVertex[i].y;
|
||||
if (ry_range.max < binRectVertex[i].y)
|
||||
ry_range.max = binRectVertex[i].y;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int x_cols = (int)((rx_range.max - rx_range.min) / scale) + 1;
|
||||
if (x_cols % 2 == 1)
|
||||
x_cols += 1;
|
||||
@ -1763,7 +1866,7 @@ void _genXOYProjectionImage_workpieceInfo(
|
||||
|
||||
cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3);
|
||||
//计算投影比例
|
||||
_XOYprojection_workpieceInfo(img, rotateLines, rotatedWorkpiece, scale, sideWidth, rx_range, ry_range, dirLen);
|
||||
_XOYprojection_workpieceInfo(img, rotateLines, rotatedWorkpiece, binRectVertex, scale, sideWidth, rx_range, ry_range, dirLen);
|
||||
cv::imwrite(fileName, img);
|
||||
return;
|
||||
}
|
||||
@ -2145,17 +2248,18 @@ void HaiRuiMa_groundCalib_test(void)
|
||||
}
|
||||
|
||||
//拓普发工件孔定位（工件定位)
|
||||
#define HRM_RotorCore_TEST_GROUP 2
|
||||
#define HRM_RotorCore_TEST_GROUP 3
|
||||
void HaiRuiMa_rotorCorePositioning_test(void)
|
||||
{
|
||||
const char* dataPath[HRM_RotorCore_TEST_GROUP] = {
|
||||
|
||||
"F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/数据2D3D/", //0
|
||||
"F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/WorkpieceRotor/", //1
|
||||
"F:/ShangGu/项目/冠钦项目/宁波海瑞马/转子钢芯/现场数据/"
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[HRM_RotorCore_TEST_GROUP] = {
|
||||
{1,10}, {3,70}
|
||||
{1,10}, {3,70},{1,1}
|
||||
};
|
||||
|
||||
const char* ver = wd_hybridPositioningVersion();
|
||||
@ -2183,7 +2287,7 @@ void HaiRuiMa_rotorCorePositioning_test(void)
|
||||
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx = 26;
|
||||
//fidx = 50;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%s%d_cloud.txt", dataPath[grp], fidx);
|
||||
std::vector<std::vector< SVzNLPositionD>> scanLines;
|
||||
@ -2199,6 +2303,7 @@ void HaiRuiMa_rotorCorePositioning_test(void)
|
||||
#endif
|
||||
|
||||
WD_HRM_BinInfo binInfo;
|
||||
#if 0
|
||||
if (grp == 1)
|
||||
{
|
||||
std::vector< std::vector<SVzNL3DPosition>> scanData;
|
||||
@ -2217,13 +2322,13 @@ void HaiRuiMa_rotorCorePositioning_test(void)
|
||||
double zSliceRange = 50.0; //取料筐最上边沿的Z切片范围
|
||||
binInfo = wd_HRM_getBinSize(scanData, groundCalibPara, binHeight, zSliceRange, &rCode);
|
||||
}
|
||||
else
|
||||
else if(grp == 0)
|
||||
{
|
||||
//读取料筐参数
|
||||
sprintf_s(calibFile, "%sbin_info.txt", dataPath[grp]);
|
||||
binInfo = _readBinInfo(calibFile);
|
||||
}
|
||||
|
||||
#endif
|
||||
sprintf_s(_scan_file, "%s%d_result.txt", dataPath[grp], fidx);
|
||||
std::vector<WD_objArea2D> objROIs;
|
||||
if( (grp == 0) && (fidx <= 9))
|
||||
@ -2235,6 +2340,7 @@ void HaiRuiMa_rotorCorePositioning_test(void)
|
||||
workpieceParam.radius = 45.0;
|
||||
workpieceParam.height = 20.0;
|
||||
|
||||
double groundRemoveOffset = 200; //料筐地面去除的Z偏置，实际地面去除高度为 groundCalibPara.planeHeight - groundRemoveOffset
|
||||
long t1 = (long)GetTickCount64();//统计时间
|
||||
|
||||
int errCode = 0;
|
||||
@ -2243,10 +2349,11 @@ void HaiRuiMa_rotorCorePositioning_test(void)
|
||||
wd_HRM_RotorCorePositioning(
|
||||
scanLines,
|
||||
objROIs,
|
||||
binInfo,
|
||||
groundCalibPara,
|
||||
workpieceParam,
|
||||
groundRemoveOffset,
|
||||
workpiecePositions,
|
||||
binInfo,
|
||||
#ifdef _OUTPUT_DEBUG_DATA
|
||||
debugScanLines,
|
||||
#endif
|
||||
@ -2277,14 +2384,14 @@ void HaiRuiMa_rotorCorePositioning_test(void)
|
||||
workpiecePositions[i].x_dir = rpt;
|
||||
}
|
||||
sprintf_s(_scan_file, "%sresult/result_%d.txt", dataPath[grp], fidx);
|
||||
_outputRGBDResult_XYZ_RGBD_workpieceInfo(_scan_file, debugScanLines, workpiecePositions, 50);
|
||||
_outputRGBDResult_XYZ_RGBD_workpieceInfo(_scan_file, debugScanLines, workpiecePositions, &binInfo, 50);
|
||||
#if 1
|
||||
sprintf_s(calibFile, "%sresult/result_%d_projection.png", dataPath[grp], fidx);
|
||||
double rpy[3] = { 0, 0, 0 }; //{ 0,-45, 0 }; //
|
||||
double angleDrawLen = 15;
|
||||
double displayScale = 0.3;
|
||||
cv::String imgName(calibFile);
|
||||
_genXOYProjectionImage_workpieceInfo(imgName, debugScanLines, displayScale, workpiecePositions, rpy, angleDrawLen);
|
||||
_genXOYProjectionImage_workpieceInfo(imgName, debugScanLines, displayScale, workpiecePositions, &binInfo, rpy, angleDrawLen);
|
||||
#endif
|
||||
printf("done\n");
|
||||
}
|
||||
@ -2487,7 +2594,7 @@ void HaiRuiMa_workpieceSizeCompute_test(void)
|
||||
a_pose.value = workpieceInfo.workpieceRadius;
|
||||
a_pose.z_dir = { 0, 0, 1.0 };
|
||||
poseInfo.push_back(a_pose);
|
||||
_outputRGBDResult_XYZ_RGBD_workpieceInfo(_scan_file, scanLines, poseInfo, 500);
|
||||
_outputRGBDResult_XYZ_RGBD_workpieceInfo(_scan_file, scanLines, poseInfo, NULL, 500);
|
||||
sprintf_s(_scan_file, "%sresult/%d_workpiece_info.txt", dataPath[grp], fidx);
|
||||
_outputWorkpieceInfo(_scan_file, workpieceInfo);
|
||||
|
||||
@ -2496,7 +2603,7 @@ void HaiRuiMa_workpieceSizeCompute_test(void)
|
||||
double angleDrawLen = 500;
|
||||
double displayScale = 1.0;
|
||||
cv::String imgName(calibFile);
|
||||
_genXOYProjectionImage_workpieceInfo(imgName, scanLines, displayScale, poseInfo, rpy, angleDrawLen);
|
||||
_genXOYProjectionImage_workpieceInfo(imgName, scanLines, displayScale, poseInfo, NULL, rpy, angleDrawLen);
|
||||
printf("done\n");
|
||||
}
|
||||
}
|
||||
|
||||
@ -19,7 +19,8 @@
|
||||
//version 1.3.6 : 在1.3.5版本的基础上根据现场数据修正了转子钢芯定位算法中的bug
|
||||
//version 1.3.7 : 在1.3.6版本的基础上添加了ID， 与2D结果保持一致
|
||||
//version 1.3.8 : 添加了输入栅格格式检查， 调整边缘目标为目标距料筐外廓边的距离在直径的1.1倍以内
|
||||
std::string m_strVersion = "HybridPositioning 1.3.8";
|
||||
//version 1.3.9 : 转子钢芯， 改进了料筐检测方法
|
||||
std::string m_strVersion = "HybridPositioning 1.3.9";
|
||||
const char* wd_hybridPositioningVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -937,13 +938,160 @@ void _getYTopLine(
|
||||
return;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
//检测料筐边缘，以中心点为基准进行规划
|
||||
WD_HRM_BinInfo wd_HRM_getBinSize_2(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SVzNLRangeD zSliceRange)
|
||||
{
|
||||
WD_HRM_BinInfo resultPose;
|
||||
memset(&resultPose, 0, sizeof(WD_HRM_BinInfo));
|
||||
|
||||
int lineNum = (int)scanLines.size();
|
||||
int linePtNum = (int)scanLines[0].size();
|
||||
|
||||
//Z层切
|
||||
std::vector<SVzNL3DPosition> zBinSliceData;
|
||||
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 > zSliceRange.min) && (scanLines[line][j].pt3D.z < zSliceRange.max))
|
||||
{
|
||||
SVzNL3DPosition a_pt;
|
||||
a_pt.nPointIdx = (line << 16) | (j & 0xffff);
|
||||
a_pt.pt3D = scanLines[line][j].pt3D;
|
||||
zBinSliceData.push_back(a_pt);
|
||||
scanLines[line][j].nPointIdx = 1; //标注
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//聚类
|
||||
int clusterCheckWin = 5;
|
||||
double clusterDist = 10.0;
|
||||
int distType = 1; //0 - 2d distance; 1- 3d distance
|
||||
std::vector<std::vector< SVzNL3DPosition>> objClusters;
|
||||
wd_pointClustering_speedUp(zBinSliceData, lineNum, linePtNum, clusterCheckWin, clusterDist, distType, objClusters);
|
||||
if(objClusters.size() == 0)
|
||||
return resultPose;
|
||||
|
||||
int bestIdx = 0;
|
||||
for (int i = 1; i < (int)objClusters.size(); i++)
|
||||
{
|
||||
if (objClusters[bestIdx].size() < objClusters[i].size())
|
||||
bestIdx = i;
|
||||
}
|
||||
|
||||
std::vector<SVzNL3DPosition>& zSliceData = objClusters[bestIdx];
|
||||
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);
|
||||
}
|
||||
if (points.size() == 0)
|
||||
{
|
||||
return resultPose;
|
||||
}
|
||||
|
||||
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 = zSliceRange.min;
|
||||
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;
|
||||
}
|
||||
|
||||
void wd_HRM_RotorCorePositioning(
|
||||
std::vector< std::vector<SVzNLPositionD>>& scanLinesInput,
|
||||
std::vector<WD_objArea2D>& objROIs,
|
||||
const WD_HRM_BinInfo binInfo,
|
||||
const SSG_planeCalibPara groundCalibPara,
|
||||
const SWD_Cylinder workpieceParam, //圆柱形工件的标称半径和高度
|
||||
const double groundRemoveOffset, //料筐地面去除的Z偏置，实际地面去除高度为 groundCalibPara.planeHeight - groundRemoveOffset
|
||||
std::vector< WD_workpieceInfo>& workpiecePositions,
|
||||
WD_HRM_BinInfo& binInfo, //输出Bin信息，用于debug，确认料筐信息检测正确
|
||||
#ifdef _OUTPUT_DEBUG_DATA
|
||||
std::vector<std::vector<SVzNL3DPosition>>& debugScanLines,
|
||||
#endif
|
||||
@ -1025,6 +1173,14 @@ void wd_HRM_RotorCorePositioning(
|
||||
wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
|
||||
}
|
||||
|
||||
//生成数据副本，用于Bin信息检测
|
||||
std::vector<std::vector<SVzNL3DPosition>> dataCopy;
|
||||
dataCopy.resize(scanLines.size());
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
dataCopy[i].insert(dataCopy[i].end(), scanLines[i].begin(), scanLines[i].end());
|
||||
}
|
||||
|
||||
//产生水平扫描数据
|
||||
std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
|
||||
scanLines_h.resize(linePtNum);
|
||||
@ -1294,6 +1450,13 @@ void wd_HRM_RotorCorePositioning(
|
||||
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;
|
||||
|
||||
//计算料筐信息
|
||||
SVzNLRangeD zSliceRange;
|
||||
zSliceRange.min = highest_z - workpieceParam.height;
|
||||
zSliceRange.max = groundCalibPara.planeHeight - groundRemoveOffset;
|
||||
binInfo = wd_HRM_getBinSize_2(dataCopy, zSliceRange);
|
||||
|
||||
//z方向排序
|
||||
std::vector< WD_workpieceInfo> zSortWorkpiece;
|
||||
double topLayerTh = highest_z + workpieceParam.height * 0.75;
|
||||
@ -1747,31 +1910,6 @@ SSG_ROIRectD _getListROI(std::vector< SVzNL3DPosition>& listData)
|
||||
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)
|
||||
{
|
||||
|
||||
@ -61,10 +61,11 @@ SG_APISHARED_EXPORT void wd_lineDataR(
|
||||
SG_APISHARED_EXPORT void wd_HRM_RotorCorePositioning(
|
||||
std::vector< std::vector<SVzNLPositionD>>& scanLinesInput,
|
||||
std::vector<WD_objArea2D>& objROIs,
|
||||
const WD_HRM_BinInfo binInfo,
|
||||
const SSG_planeCalibPara groundCalibPara,
|
||||
const SWD_Cylinder workpieceParam, //圆柱形工件的标称半径和高度
|
||||
const double groundRemoveOffset, //料筐地面去除的Z偏置，实际地面去除高度为 groundCalibPara.planeHeight - groundRemoveOffset
|
||||
std::vector< WD_workpieceInfo>& workpiecePositions,
|
||||
WD_HRM_BinInfo& binInfo, //输出Bin信息，用于debug，确认料筐信息检测正确
|
||||
#ifdef _OUTPUT_DEBUG_DATA
|
||||
std::vector<std::vector<SVzNL3DPosition>>& debugScanLines,
|
||||
#endif
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user