rodAndBarDetection
version 1.6.0 : 矩森棒材抓取算法改进：添加了料筐信息提取和对边缘工件的判断
This commit is contained in:
parent
dc667b4d34
commit
6674c32adc
@ -680,7 +680,8 @@ void _outputRGBDScan_RGBD_centerPose(
|
||||
void _outputRGBDScan_RGBD_rodInfo(
|
||||
char* fileName,
|
||||
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
|
||||
std::vector<SSX_rodPositionInfo>& rodInfo
|
||||
std::vector<SSX_rodPositionInfo>& rodInfo,
|
||||
SWD_BinInfo* binInfo
|
||||
)
|
||||
{
|
||||
int lineNum = (int)scanLines.size();
|
||||
@ -735,11 +736,16 @@ void _outputRGBDScan_RGBD_rodInfo(
|
||||
rgb = { 180, 0, 0 };
|
||||
size = 2;
|
||||
}
|
||||
else
|
||||
else if(pt3D->nPointIdx < 3)
|
||||
{
|
||||
rgb = objColor[pt3D->nPointIdx % 8];
|
||||
size = 2;
|
||||
}
|
||||
else //料筐
|
||||
{
|
||||
rgb = { 0, 200,200 };
|
||||
size = 2;
|
||||
}
|
||||
}
|
||||
else //if (pt3D->nPointIdx == 0)
|
||||
{
|
||||
@ -761,11 +767,13 @@ void _outputRGBDScan_RGBD_rodInfo(
|
||||
if (objNum < (int)scanLines[0].size())
|
||||
lastLineNum = (int)scanLines[0].size();
|
||||
sw << "Line_" << lineIdx << "_0_" << lastLineNum << std::endl;
|
||||
size = 12;
|
||||
size = 25;
|
||||
for (int i = 0; i < objNum; i++)
|
||||
{
|
||||
if(i == 0)
|
||||
rgb = { 250, 255, 0 };
|
||||
if (i == 0)
|
||||
rgb = { 250, 250, 0 };
|
||||
else if (rodInfo[i].workpieceType > 0) //边上目标
|
||||
rgb = { 0, 250, 250 };
|
||||
else
|
||||
rgb = { 250, 0, 0 };
|
||||
float x = (float)rodInfo[i].center.x;
|
||||
@ -816,6 +824,55 @@ void _outputRGBDScan_RGBD_rodInfo(
|
||||
lineIdx++;
|
||||
}
|
||||
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();
|
||||
}
|
||||
@ -1323,14 +1380,38 @@ void _convertToGridData_XYZRGB(std::vector<std::vector< SVzNLPointXYZRGBA>>& sca
|
||||
return;
|
||||
}
|
||||
|
||||
void rotationMatrixToEulerZYX1(const double R[9], double rpy[3]) {
|
||||
// 计算俯仰角（pitch）θ
|
||||
double pitch = asin(-R[6]); // asin返回弧度
|
||||
double yaw, roll;
|
||||
// 检查万向节锁（cosθ接近0）
|
||||
const double epsilon = 1e-6;
|
||||
if (abs(cos(pitch)) > epsilon) {
|
||||
// 无万向节锁，正常计算yaw和roll
|
||||
yaw = atan2(R[3], R[0]);
|
||||
roll = atan2(R[7], R[8]);
|
||||
}
|
||||
else {
|
||||
// 万向节锁，约定roll=0，计算yaw
|
||||
roll = 0.0;
|
||||
yaw = atan2(-R[1], R[4]);
|
||||
}
|
||||
|
||||
// 将弧度转换为角度
|
||||
const double rad2deg = 180.0 / M_PI;
|
||||
rpy[2] = yaw* rad2deg;
|
||||
rpy[1] = pitch* rad2deg;
|
||||
rpy[0] = roll* rad2deg;
|
||||
return;
|
||||
}
|
||||
void EulerRpyToRotation1(const double rpy[3], double matrix3d[9]) {
|
||||
double cos0 = cos(rpy[0] * PI / 180);
|
||||
double cos0 = cos(rpy[0] * PI / 180);
|
||||
double sin0 = sin(rpy[0] * PI / 180);
|
||||
double cos1 = cos(rpy[1] * PI / 180);
|
||||
double sin1 = sin(rpy[1] * PI / 180);
|
||||
double cos2 = cos(rpy[2] * PI / 180);
|
||||
double sin2 = sin(rpy[2] * PI / 180);
|
||||
matrix3d[0] = cos2 * cos1;
|
||||
matrix3d[0] = cos2 * cos1;
|
||||
matrix3d[1] = cos2 * sin1 * sin0 - sin2 * cos0;
|
||||
matrix3d[2] = cos2 * sin1 * cos0 + sin2 * sin0;
|
||||
matrix3d[3] = sin2 * cos1;
|
||||
@ -1406,6 +1487,7 @@ void _XOYprojection_rodInfo(
|
||||
cv::Mat& img,
|
||||
std::vector<std::vector< SVzNL3DPosition>>& dataLines,
|
||||
std::vector<SSX_rodPositionInfo>& rodInfo,
|
||||
std::vector<SVzNL3DPoint>& binRectVertex,
|
||||
const double scale,
|
||||
const int sideWidth,
|
||||
const SVzNLRangeD x_range,
|
||||
@ -1441,11 +1523,16 @@ void _XOYprojection_rodInfo(
|
||||
rgb = { 180, 0, 0 };
|
||||
size = 2;
|
||||
}
|
||||
else
|
||||
else if(pt3D.nPointIdx < 3)
|
||||
{
|
||||
rgb = objColor[pt3D.nPointIdx % 8];
|
||||
size = 2;
|
||||
}
|
||||
else //料筐
|
||||
{
|
||||
rgb = { 0, 200, 200 };
|
||||
size = 2;
|
||||
}
|
||||
}
|
||||
else //if (pt3D->nPointIdx == 0)
|
||||
{
|
||||
@ -1464,11 +1551,12 @@ void _XOYprojection_rodInfo(
|
||||
}
|
||||
if (rodInfo.size() > 0)
|
||||
{
|
||||
|
||||
for (int i = 0; i < (int)rodInfo.size(); i++)
|
||||
{
|
||||
if (i == 0)
|
||||
rgb = { 255, 255, 0 };
|
||||
else if (rodInfo[i].workpieceType > 0) //边上目标
|
||||
rgb = { 0, 250, 250 };
|
||||
else
|
||||
rgb = { 255, 0, 0 };
|
||||
size = int(10.0 / scale);
|
||||
@ -1486,6 +1574,24 @@ void _XOYprojection_rodInfo(
|
||||
cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), size);
|
||||
}
|
||||
}
|
||||
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 };
|
||||
cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void _genXOYProjectionImage_rodInfo(
|
||||
@ -1493,6 +1599,7 @@ void _genXOYProjectionImage_rodInfo(
|
||||
std::vector<std::vector< SVzNL3DPosition>>& scanLines,
|
||||
const double scale,
|
||||
std::vector<SSX_rodPositionInfo>& rodInfo,
|
||||
SWD_BinInfo* binInfo,
|
||||
const double rpy[3], const double dirLen)
|
||||
{
|
||||
//旋转视角显示
|
||||
@ -1503,6 +1610,8 @@ void _genXOYProjectionImage_rodInfo(
|
||||
for (int i = 0; i < (int)rodInfo.size(); i++)
|
||||
{
|
||||
SSX_rodPositionInfo rotateObj;
|
||||
rotateObj.id = rodInfo[i].id;
|
||||
rotateObj.workpieceType = rodInfo[i].workpieceType;
|
||||
rotateObj.center = _ptRotate(rodInfo[i].center, matrix3d);
|
||||
rotateObj.axialDir = _ptRotate(rodInfo[i].axialDir, matrix3d);
|
||||
rotateObj.normalDir = _ptRotate(rodInfo[i].normalDir, matrix3d);
|
||||
@ -1515,6 +1624,40 @@ void _genXOYProjectionImage_rodInfo(
|
||||
SVzNLRangeD rx_range, ry_range;
|
||||
_rotateCloudPts(scanLines, matrix3d, rotateLines, rx_range, ry_range);
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//统计X和Y的范围
|
||||
int x_cols = (int)((rx_range.max - rx_range.min) / scale) + 1;
|
||||
if (x_cols % 2 == 1)
|
||||
@ -1529,12 +1672,12 @@ void _genXOYProjectionImage_rodInfo(
|
||||
|
||||
cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3);
|
||||
//计算投影比例
|
||||
_XOYprojection_rodInfo(img, rotateLines, rotatedRod, scale, sideWidth, rx_range, ry_range, dirLen);
|
||||
_XOYprojection_rodInfo(img, rotateLines, rotatedRod, binRectVertex, scale, sideWidth, rx_range, ry_range, dirLen);
|
||||
cv::imwrite(fileName, img);
|
||||
return;
|
||||
}
|
||||
|
||||
#define ROD_POSITION_TEST_GROUP 7
|
||||
#define ROD_POSITION_TEST_GROUP 8
|
||||
#define TEST_CONVERT_TO_GRID 0
|
||||
#define TEST_COMPUTE_CALIB_PARA 0
|
||||
#define TEST_COMPUTE_ROD_POSITION 1
|
||||
@ -1641,17 +1784,18 @@ void rodPositionTest(void)
|
||||
|
||||
"F:/ShangGu/项目/冠钦项目/矩森棒材抓取/异常点云/", //5
|
||||
"F:/ShangGu/项目/冠钦项目/矩森棒材抓取/异常点云2/", //6
|
||||
"F:/ShangGu/项目/冠钦项目/矩森棒材抓取/异常点云3/", //7
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[ROD_POSITION_TEST_GROUP] = {
|
||||
{1,8}, {1,3}, {1,31},
|
||||
{1,5},{1,11},{1,26},{1,41}
|
||||
{1,5},{1,11},{1,26},{1,41}, {1, 23}
|
||||
};
|
||||
|
||||
const char* ver = wd_rodAndBarDetectionVersion();
|
||||
printf("ver:%s\n", ver);
|
||||
|
||||
for (int grp = 0; grp <= 6; grp++)
|
||||
for (int grp = 7; grp <= 7; grp++)
|
||||
{
|
||||
SSG_planeCalibPara poseCalibPara;
|
||||
//初始化成单位阵
|
||||
@ -1687,6 +1831,8 @@ void rodPositionTest(void)
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_Ik256066.txt", dataPath[grp], fidx);
|
||||
else if (6 == grp)
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_Ik256066.txt", dataPath[grp], fidx);
|
||||
else if(7 == grp)
|
||||
sprintf_s(_scan_file, "%s%d_cloud.txt", dataPath[grp], fidx);
|
||||
else
|
||||
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
|
||||
|
||||
@ -1709,7 +1855,7 @@ void rodPositionTest(void)
|
||||
rodParam.diameter = 96.0; //圆棒直径
|
||||
rodParam.len = 440;
|
||||
}
|
||||
else if (grp == 6)
|
||||
else if ( (grp == 6) ||(grp ==7))
|
||||
{
|
||||
rodParam.diameter = 62.0; //圆棒直径
|
||||
rodParam.len = 380;
|
||||
@ -1740,9 +1886,18 @@ void rodPositionTest(void)
|
||||
growParam.minLTypeTreeLen = rodParam.len * 0.75; //mm, 螺杆长度
|
||||
growParam.minVTypeTreeLen = rodParam.len * 0.75; //mm
|
||||
|
||||
bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道
|
||||
const double distToSideRatio = 1.1; //边距系数，边距门限=边距系数*工件直径；当工件距边的距离大于边距门限时，判断为边上的工件
|
||||
double groundRemoveOffset = 100; //料筐地面去除的Z偏置，实际地面去除高度为 groundCalibPara.planeHeight - groundRemoveOffset
|
||||
double clusterNDist; //料筐聚类尺度， 当两点距离小于此尺度时， 被聚为一类。此参数和点云密度相关，用于滤除料筐周围的干扰。
|
||||
if ( grp == 5)
|
||||
clusterNDist = 10.0;// 10.0;
|
||||
else if (grp == 6)
|
||||
clusterNDist = 15.0;
|
||||
else if (grp == 7)
|
||||
clusterNDist = 15.0;
|
||||
int errCode = 0;
|
||||
std::vector<SSX_rodPositionInfo> rodInfo;
|
||||
SWD_BinInfo binInfo;
|
||||
sx_rodPositioning(
|
||||
scanLines,
|
||||
poseCalibPara,
|
||||
@ -1750,14 +1905,18 @@ void rodPositionTest(void)
|
||||
filterParam,
|
||||
growParam,
|
||||
rodParam,
|
||||
distToSideRatio,
|
||||
groundRemoveOffset,
|
||||
clusterNDist,
|
||||
rodInfo,
|
||||
binInfo,
|
||||
&errCode);
|
||||
|
||||
long t2 = (long)GetTickCount64();
|
||||
printf("%s: %d(ms),errCode=%d\n", _scan_file, (int)(t2 - t1),errCode);
|
||||
//输出测试结果
|
||||
sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx);
|
||||
_outputRGBDScan_RGBD_rodInfo(_scan_file, scanLines, rodInfo);
|
||||
_outputRGBDScan_RGBD_rodInfo(_scan_file, scanLines, rodInfo, &binInfo);
|
||||
sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx);
|
||||
_outputRodtInfo(_scan_file, rodInfo);
|
||||
#if 1
|
||||
@ -1766,7 +1925,12 @@ void rodPositionTest(void)
|
||||
double angleDrawLen = 15;
|
||||
double displayScale = 1.0;
|
||||
cv::String imgName(_scan_file);
|
||||
_genXOYProjectionImage_rodInfo(imgName, scanLines, displayScale, rodInfo, rpy, angleDrawLen);
|
||||
_genXOYProjectionImage_rodInfo(imgName, scanLines, displayScale, rodInfo, &binInfo, rpy, angleDrawLen);
|
||||
//正投影
|
||||
sprintf_s(_scan_file, "%sresult/%d_result_projection_2.png", dataPath[grp], fidx);
|
||||
rotationMatrixToEulerZYX1(poseCalibPara.planeCalib, rpy);
|
||||
cv::String imgName2(_scan_file);
|
||||
_genXOYProjectionImage_rodInfo(imgName2, scanLines, displayScale, rodInfo, &binInfo, rpy, angleDrawLen);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
@ -731,3 +731,16 @@ typedef struct
|
||||
SVzNL3DRangeD roi3D;
|
||||
SVzNLRect roi2D;
|
||||
}SWD_clustersInfo;
|
||||
|
||||
//料筐信息
|
||||
typedef struct
|
||||
{
|
||||
double length;
|
||||
double width;
|
||||
double binTopZ; //料筐最高处的Z值
|
||||
SVzNL3DPoint center; //中心坐标
|
||||
SVzNL3DPoint bottomNormal; //底面的法向
|
||||
SVzNL3DPoint x_dir; //长轴与X轴的倾斜角，
|
||||
SVzNL3DPoint y_dir; //长轴与X轴的倾斜角，
|
||||
SVzNL3DPoint minRectVertex[4]; //最小外接矩形顶点
|
||||
}SWD_BinInfo;
|
||||
@ -31,7 +31,8 @@
|
||||
//version 1.3.8 : 新的螺杆定位算法，使用PCA方法确定螺杆轴向
|
||||
//version 1.4.0 : 矩森棒材抓取算法改进：（1）修正了两个棒材被识别成一根的问题（2）增强了对噪点的容忍度，增强了鲁棒性
|
||||
//version 1.5.0 : 矩森棒材抓取算法改进：加强了对强噪声点云的处理
|
||||
std::string m_strVersion = "RodAndBarDetection_1.5.0";
|
||||
//version 1.6.0 : 矩森棒材抓取算法改进：添加了料筐信息提取和对边缘工件的判断
|
||||
std::string m_strVersion = "RodAndBarDetection_1.6.0";
|
||||
const char* wd_rodAndBarDetectionVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -243,6 +244,164 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
|
||||
}
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
//检测料筐边缘，以聚类为基础，取最大聚类作为料筐，以中心点为基准进行规划
|
||||
SWD_BinInfo wd_cluster_getBinSize(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SVzNLRangeD zSliceRange,
|
||||
const double clusterNDist,
|
||||
std::vector<SVzNL3DPosition>& binPoints)
|
||||
{
|
||||
SWD_BinInfo resultPose;
|
||||
memset(&resultPose, 0, sizeof(SWD_BinInfo));
|
||||
|
||||
int lineNum = (int)scanLines.size();
|
||||
int linePtNum = (int)scanLines[0].size();
|
||||
|
||||
//去除单独噪点
|
||||
SSG_outlierFilterParam filterParam;
|
||||
filterParam.continuityTh = 5.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
|
||||
filterParam.outlierTh = 3;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
wd_vectorDataRemoveOutlier_overwrite(scanLines[line], filterParam);
|
||||
|
||||
//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 = 6;
|
||||
double clusterDist = clusterNDist;
|
||||
int distType = 0; //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;
|
||||
}
|
||||
|
||||
//输出料筐点云，debug
|
||||
binPoints.insert(binPoints.end(), zSliceData.begin(), zSliceData.end());
|
||||
|
||||
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 rodArcFeatueDetection(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SSG_cornerParam cornerPara,
|
||||
@ -1596,31 +1755,6 @@ void sx_hexHeadScrewMeasure_PCA(
|
||||
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;
|
||||
}
|
||||
|
||||
SSG_ROIRectD _getListROI(std::vector< SVzNL3DPosition>& listData)
|
||||
{
|
||||
if (listData.size() == 0)
|
||||
@ -3389,7 +3523,11 @@ void sx_rodPositioning(
|
||||
const SSG_outlierFilterParam filterParam,
|
||||
const SSG_treeGrowParam growParam,
|
||||
const SSX_rodParam rodParam,
|
||||
const double distToSideRatio, //边距系数，边距门限=边距系数*工件直径；当工件距边的距离大于边距门限时，判断为边上的工件
|
||||
const double groundRemoveOffset, //料筐地面去除的Z偏置，实际地面去除高度为 groundCalibPara.planeHeight - groundRemoveOffset
|
||||
const double clusterNDist, //料筐聚类尺度， 当两点距离小于此尺度时， 被聚为一类。此参数和点云密度相关，用于滤除料筐周围的干扰。
|
||||
std::vector<SSX_rodPositionInfo>& rodInfo,
|
||||
SWD_BinInfo& binInfo, //输出Bin信息，用于debug，确认料筐信息检测正确
|
||||
int* errCode)
|
||||
{
|
||||
*errCode = 0;
|
||||
@ -3428,6 +3566,14 @@ void sx_rodPositioning(
|
||||
sx_rodPosition_lineDataR(scanLines[i], poseCalibPara.planeCalib, cuttingZ);
|
||||
}
|
||||
|
||||
//生成数据副本，用于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>> hLines_raw;
|
||||
hLines_raw.resize(linePtNum);
|
||||
@ -3691,6 +3837,7 @@ void sx_rodPositioning(
|
||||
if (fittingPoints.size() < 3)
|
||||
continue;
|
||||
SSX_rodPositionInfo a_objRod;
|
||||
memset(&a_objRod, 0, sizeof(SSX_rodPositionInfo));
|
||||
_computeRodInfo(rodArcTrees_v[i], segEndings[m].nMin, segEndings[m].nMax, false, fittingPoints, a_objRod);
|
||||
rodInfo.push_back(a_objRod);
|
||||
}
|
||||
@ -3760,6 +3907,7 @@ void sx_rodPositioning(
|
||||
continue;
|
||||
|
||||
SSX_rodPositionInfo a_objRod;
|
||||
memset(&a_objRod, 0, sizeof(SSX_rodPositionInfo));
|
||||
_computeRodInfo(rodArcTrees_h[i], segEndings[m].nMin, segEndings[m].nMax, true, fittingPoints, a_objRod);
|
||||
//检查是否与垂直检测目标重叠
|
||||
bool isExist = checkObjEixst(a_objRod, rodInfo, rodParam);
|
||||
@ -3772,9 +3920,162 @@ void sx_rodPositioning(
|
||||
//按高度排序
|
||||
std::sort(rodInfo.begin(), rodInfo.end(), _commpareByCenterZ);
|
||||
|
||||
//计算料筐信息
|
||||
double highest_z = rodInfo[0].center.z;
|
||||
SVzNLRangeD zSliceRange;
|
||||
zSliceRange.min = highest_z - rodParam.diameter * 0.5;
|
||||
double groundTh = poseCalibPara.planeHeight - groundRemoveOffset;
|
||||
#if 1
|
||||
zSliceRange.max = highest_z + rodParam.diameter * 4.5;
|
||||
if (zSliceRange.max > groundTh)
|
||||
zSliceRange.max = groundTh;
|
||||
#else
|
||||
zSliceRange.max = groundTh;
|
||||
#endif
|
||||
|
||||
std::vector<SVzNL3DPosition> binPoints;
|
||||
binInfo = wd_cluster_getBinSize(dataCopy, zSliceRange, clusterNDist,binPoints);
|
||||
//标注料筐数据，debug
|
||||
for (int i = 0; i < (int)binPoints.size(); i++)
|
||||
{
|
||||
int lineIdx = binPoints[i].nPointIdx >> 16;
|
||||
int ptIdx = binPoints[i].nPointIdx & 0xffff;
|
||||
if (scanLines[lineIdx][ptIdx].nPointIdx == 0)
|
||||
{
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 3;
|
||||
scanLines[lineIdx][ptIdx].pt3D = binPoints[i].pt3D; //将可能的被滤除的点恢复
|
||||
}
|
||||
}
|
||||
|
||||
if (binInfo.binTopZ > 1e-4)
|
||||
{
|
||||
//计算工件位置属性
|
||||
std::vector<SWD_3DPointPair> linePoint;
|
||||
linePoint.resize(4);
|
||||
linePoint[0].pt1 = binInfo.minRectVertex[0];
|
||||
linePoint[0].pt2 = binInfo.minRectVertex[1];
|
||||
linePoint[1].pt1 = binInfo.minRectVertex[1];
|
||||
linePoint[1].pt2 = binInfo.minRectVertex[2];
|
||||
linePoint[2].pt1 = binInfo.minRectVertex[2];
|
||||
linePoint[2].pt2 = binInfo.minRectVertex[3];
|
||||
linePoint[3].pt1 = binInfo.minRectVertex[3];
|
||||
linePoint[3].pt2 = binInfo.minRectVertex[0];
|
||||
int lineTopIdx = -1;
|
||||
for (int i = 0; i < (int)linePoint.size(); i++)
|
||||
{
|
||||
if ((linePoint[i].pt1.y < binInfo.center.y) && (linePoint[i].pt2.y < binInfo.center.y))
|
||||
{
|
||||
lineTopIdx = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (lineTopIdx < 0)
|
||||
{
|
||||
*errCode = SX_ERR_BIN_EDGE_ERROR;
|
||||
}
|
||||
SWD_3DPointPair lineTop = linePoint[lineTopIdx];
|
||||
linePoint.erase(linePoint.begin() + lineTopIdx);//去掉已经识别的edge
|
||||
|
||||
int lineBtmIdx = -1;
|
||||
for (int i = 0; i < (int)linePoint.size(); i++)
|
||||
{
|
||||
if ((linePoint[i].pt1.y > binInfo.center.y) && (linePoint[i].pt2.y > binInfo.center.y))
|
||||
{
|
||||
lineBtmIdx = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (lineBtmIdx < 0)
|
||||
{
|
||||
*errCode = SX_ERR_BIN_EDGE_ERROR;
|
||||
}
|
||||
SWD_3DPointPair lineBtm = linePoint[lineBtmIdx];
|
||||
linePoint.erase(linePoint.begin() + lineBtmIdx); //去掉已经识别的edge
|
||||
|
||||
int lineLeftIdx = -1;
|
||||
for (int i = 0; i < (int)linePoint.size(); i++)
|
||||
{
|
||||
if ((linePoint[i].pt1.x < binInfo.center.x) && (linePoint[i].pt2.x < binInfo.center.x))
|
||||
{
|
||||
lineLeftIdx = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (lineLeftIdx < 0)
|
||||
{
|
||||
*errCode = SX_ERR_BIN_EDGE_ERROR;
|
||||
}
|
||||
SWD_3DPointPair lineLeft = linePoint[lineLeftIdx];
|
||||
linePoint.erase(linePoint.begin() + lineLeftIdx); //去掉已经识别的edge
|
||||
|
||||
int lineRightIdx = -1;
|
||||
for (int i = 0; i < (int)linePoint.size(); i++)
|
||||
{
|
||||
if ((linePoint[i].pt1.x > binInfo.center.x) && (linePoint[i].pt2.x > binInfo.center.x))
|
||||
{
|
||||
lineRightIdx = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (lineRightIdx < 0)
|
||||
{
|
||||
*errCode = SX_ERR_BIN_EDGE_ERROR;
|
||||
}
|
||||
if (*errCode == 0)
|
||||
{
|
||||
SWD_3DPointPair lineRight = linePoint[lineRightIdx];
|
||||
//计算工件的位置：左上、右上、左下、右下四个角；上下左右四条边：中央部分
|
||||
//机械臂对不同部分的工件会采用不同的抓取策略
|
||||
double distToSideTh = rodParam.diameter * distToSideRatio;
|
||||
double lineTop_a, lineTop_b, lineTop_c;
|
||||
compute2ptLine(lineTop.pt1, lineTop.pt2, &lineTop_a, &lineTop_b, &lineTop_c);
|
||||
double lineBtm_a, lineBtm_b, lineBtm_c;
|
||||
compute2ptLine(lineBtm.pt1, lineBtm.pt2, &lineBtm_a, &lineBtm_b, &lineBtm_c);
|
||||
double lineLeft_a, lineLeft_b, lineLeft_c;
|
||||
compute2ptLine(lineLeft.pt1, lineLeft.pt2, &lineLeft_a, &lineLeft_b, &lineLeft_c);
|
||||
double lineRight_a, lineRight_b, lineRight_c;
|
||||
compute2ptLine(lineRight.pt1, lineRight.pt2, &lineRight_a, &lineRight_b, &lineRight_c);
|
||||
for (int i = 0; i < (int)rodInfo.size(); i++) //对上层目标进行分析
|
||||
{
|
||||
SVzNL3DPoint& objCenter = rodInfo[i].center;
|
||||
double distTop = computePtDistToLine(objCenter.x, objCenter.y, lineTop_a, lineTop_b, lineTop_c);
|
||||
double distBtm = computePtDistToLine(objCenter.x, objCenter.y, lineBtm_a, lineBtm_b, lineBtm_c);
|
||||
double distLeft = computePtDistToLine(objCenter.x, objCenter.y, lineLeft_a, lineLeft_b, lineLeft_c);
|
||||
double distRight = computePtDistToLine(objCenter.x, objCenter.y, lineRight_a, lineRight_b, lineRight_c);
|
||||
|
||||
if (distTop < distToSideTh) //上
|
||||
{
|
||||
if (distLeft < distToSideTh)
|
||||
rodInfo[i].workpieceType = 5; //左上
|
||||
else if (distRight < distToSideTh)
|
||||
rodInfo[i].workpieceType = 6; //右上
|
||||
else
|
||||
rodInfo[i].workpieceType = 1; //上
|
||||
}
|
||||
else if (distBtm < distToSideTh) //下
|
||||
{
|
||||
if (distLeft < distToSideTh)
|
||||
rodInfo[i].workpieceType = 7; //左下
|
||||
else if (distRight < distToSideTh)
|
||||
rodInfo[i].workpieceType = 8; //右下
|
||||
else
|
||||
rodInfo[i].workpieceType = 2; //下
|
||||
}
|
||||
else if (distLeft < distToSideTh)
|
||||
rodInfo[i].workpieceType = 3; //左
|
||||
else if (distRight < distToSideTh)
|
||||
rodInfo[i].workpieceType = 4; //右
|
||||
else
|
||||
rodInfo[i].workpieceType = 0; //中央
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//将数据重新投射回原来的坐标系，以保持手眼标定结果正确
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
sx_rodPosition_lineDataR(scanLines[i], poseCalibPara.invRMatrix, -1);
|
||||
|
||||
//将检测结果重新投射回原来的坐标系
|
||||
for (int i = 0; i < (int)rodInfo.size(); i++)
|
||||
{
|
||||
@ -3791,6 +4092,17 @@ void sx_rodPositioning(
|
||||
rawObj = _translatePoint(a_rod.endPt, poseCalibPara.invRMatrix);
|
||||
a_rod.endPt = rawObj;
|
||||
}
|
||||
|
||||
if (binInfo.binTopZ > 1e-4)
|
||||
{
|
||||
//bin旋转回原坐标系
|
||||
binInfo.center = _translatePoint(binInfo.center, poseCalibPara.invRMatrix);
|
||||
binInfo.bottomNormal = _translatePoint(binInfo.bottomNormal, poseCalibPara.invRMatrix);
|
||||
binInfo.x_dir = _translatePoint(binInfo.x_dir, poseCalibPara.invRMatrix);
|
||||
binInfo.y_dir = _translatePoint(binInfo.y_dir, poseCalibPara.invRMatrix);
|
||||
for (int i = 0; i < 4; i++)
|
||||
binInfo.minRectVertex[i] = _translatePoint(binInfo.minRectVertex[i], poseCalibPara.invRMatrix);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
@ -29,6 +29,8 @@ typedef struct
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int id; //序号从1开始
|
||||
int workpieceType; //0:中央; 1-上边; 2-下边; 3-左边; 4-右边; 5-左上角; 6-右上角; 7-左下角; 8-右下角;
|
||||
SVzNL3DPoint center; //螺杆端部中心点
|
||||
SVzNL3DPoint axialDir; //轴向向量
|
||||
SVzNL3DPoint normalDir; //法向量
|
||||
@ -123,7 +125,11 @@ SG_APISHARED_EXPORT void sx_rodPositioning(
|
||||
const SSG_outlierFilterParam filterParam,
|
||||
const SSG_treeGrowParam growParam,
|
||||
const SSX_rodParam rodParam,
|
||||
const double distToSideRatio, //边距系数，边距门限=边距系数*工件直径；当工件距边的距离大于边距门限时，判断为边上的工件
|
||||
const double groundRemoveOffset, //料筐地面去除的Z偏置，实际地面去除高度为 groundCalibPara.planeHeight - groundRemoveOffset
|
||||
const double clusterNDist, //料筐聚类尺度， 当两点距离小于此尺度时， 被聚为一类。此参数和点云密度相关，用于滤除料筐周围的干扰。
|
||||
std::vector<SSX_rodPositionInfo>& rodInfo,
|
||||
SWD_BinInfo& binInfo, //输出Bin信息，用于debug，确认料筐信息检测正确
|
||||
int* errCode);
|
||||
|
||||
//筑裕钢结构钢筋焊缝定位
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user