rodAndBarDetection
version 1.5.0 : 矩森棒材抓取算法改进：加强了对强噪声点云的处理
This commit is contained in:
parent
0dc9a4904e
commit
8d49089a57
@ -724,6 +724,9 @@ void _outputRGBDScan_RGBD_rodInfo(
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
SVzNL3DPosition* pt3D = &scanLines[line][i];
|
||||
if (pt3D->pt3D.z < 1e-4)
|
||||
pt3D->pt3D = { 0.0, 0.0, 0.0 };
|
||||
|
||||
if (pt3D->nPointIdx > 0)
|
||||
{
|
||||
int centerFlag = pt3D->nPointIdx >> 4;
|
||||
@ -737,8 +740,6 @@ void _outputRGBDScan_RGBD_rodInfo(
|
||||
rgb = objColor[pt3D->nPointIdx % 8];
|
||||
size = 2;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
else //if (pt3D->nPointIdx == 0)
|
||||
{
|
||||
@ -756,7 +757,10 @@ void _outputRGBDScan_RGBD_rodInfo(
|
||||
|
||||
if (objNum > 0)
|
||||
{
|
||||
sw << "Line_" << lineIdx << "_0_" << objNum << std::endl;
|
||||
int lastLineNum = objNum;
|
||||
if (objNum < (int)scanLines[0].size())
|
||||
lastLineNum = (int)scanLines[0].size();
|
||||
sw << "Line_" << lineIdx << "_0_" << lastLineNum << std::endl;
|
||||
size = 12;
|
||||
for (int i = 0; i < objNum; i++)
|
||||
{
|
||||
@ -771,6 +775,10 @@ void _outputRGBDScan_RGBD_rodInfo(
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
for (int i = objNum; i < (int)scanLines[0].size(); i++)
|
||||
{
|
||||
sw << "{0,0,0}-{0,0}-{0,0}-{0,0,0,1 }" << std::endl;
|
||||
}
|
||||
//输出法向
|
||||
size = 8;
|
||||
double len = 60;
|
||||
@ -1315,7 +1323,218 @@ void _convertToGridData_XYZRGB(std::vector<std::vector< SVzNLPointXYZRGBA>>& sca
|
||||
return;
|
||||
}
|
||||
|
||||
#define ROD_POSITION_TEST_GROUP 6
|
||||
void EulerRpyToRotation1(const double rpy[3], double matrix3d[9]) {
|
||||
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[1] = cos2 * sin1 * sin0 - sin2 * cos0;
|
||||
matrix3d[2] = cos2 * sin1 * cos0 + sin2 * sin0;
|
||||
matrix3d[3] = sin2 * cos1;
|
||||
matrix3d[4] = sin2 * sin1 * sin0 + cos2 * cos0;
|
||||
matrix3d[5] = sin2 * sin1 * cos0 - cos2 * sin0;
|
||||
matrix3d[6] = -sin1;
|
||||
matrix3d[7] = cos1 * sin0;
|
||||
matrix3d[8] = cos1 * cos0;
|
||||
return;
|
||||
}
|
||||
|
||||
SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
|
||||
{
|
||||
SVzNL3DPoint _r_pt;
|
||||
_r_pt.x = pt3D.x * matrix3d[0] + pt3D.y * matrix3d[1] + pt3D.z * matrix3d[2];
|
||||
_r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5];
|
||||
_r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8];
|
||||
return _r_pt;
|
||||
}
|
||||
void _rotateCloudPts(
|
||||
std::vector<std::vector< SVzNL3DPosition>>& scanLines,
|
||||
double matrix3d[9],
|
||||
std::vector<std::vector< SVzNL3DPosition>>& rotateLines,
|
||||
SVzNLRangeD& rx_range, SVzNLRangeD& ry_range)
|
||||
{
|
||||
int lineNum = (int)scanLines.size();
|
||||
rotateLines.resize(lineNum);
|
||||
rx_range.min = 0;
|
||||
rx_range.max = -1;
|
||||
ry_range.min = 0;
|
||||
ry_range.max = -1;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int i = 0; i < (int)scanLines[line].size(); i++)
|
||||
{
|
||||
SVzNL3DPosition& pt3D = scanLines[line][i];
|
||||
if (pt3D.pt3D.z < 1e-4)
|
||||
continue;
|
||||
|
||||
SVzNL3DPosition r_pt;
|
||||
r_pt.pt3D = _ptRotate(pt3D.pt3D, matrix3d);
|
||||
r_pt.nPointIdx = pt3D.nPointIdx;
|
||||
rotateLines[line].push_back(r_pt);
|
||||
if (rx_range.max < rx_range.min)
|
||||
{
|
||||
rx_range.min = r_pt.pt3D.x;
|
||||
rx_range.max = r_pt.pt3D.x;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (rx_range.min > r_pt.pt3D.x)
|
||||
rx_range.min = r_pt.pt3D.x;
|
||||
if (rx_range.max < r_pt.pt3D.x)
|
||||
rx_range.max = r_pt.pt3D.x;
|
||||
}
|
||||
if (ry_range.max < ry_range.min)
|
||||
{
|
||||
ry_range.min = r_pt.pt3D.y;
|
||||
ry_range.max = r_pt.pt3D.y;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (ry_range.min > r_pt.pt3D.y)
|
||||
ry_range.min = r_pt.pt3D.y;
|
||||
if (ry_range.max < r_pt.pt3D.y)
|
||||
ry_range.max = r_pt.pt3D.y;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void _XOYprojection_rodInfo(
|
||||
cv::Mat& img,
|
||||
std::vector<std::vector< SVzNL3DPosition>>& dataLines,
|
||||
std::vector<SSX_rodPositionInfo>& rodInfo,
|
||||
const double scale,
|
||||
const int sideWidth,
|
||||
const SVzNLRangeD x_range,
|
||||
const SVzNLRangeD y_range,
|
||||
const double dirAngleLen)
|
||||
{
|
||||
cv::Vec3b rgb = cv::Vec3b(0, 0, 0);
|
||||
cv::Vec3b objColor[8] = {
|
||||
{245,222,179},//淡黄色
|
||||
{210,105, 30},//巧克力色
|
||||
{240,230,140},//黄褐色
|
||||
{135,206,235},//天蓝色
|
||||
{250,235,215},//古董白
|
||||
{189,252,201},//薄荷色
|
||||
{221,160,221},//梅红色
|
||||
{188,143,143},//玫瑰红色
|
||||
};
|
||||
int size = 1;
|
||||
for (int line = 0; line < dataLines.size(); line++)
|
||||
{
|
||||
std::vector< SVzNL3DPosition>& a_line = dataLines[line];
|
||||
for (int i = 0; i < a_line.size(); i++)
|
||||
{
|
||||
SVzNL3DPosition& pt3D = a_line[i];
|
||||
if (pt3D.pt3D.z < 1e-4)
|
||||
continue;
|
||||
|
||||
if (pt3D.nPointIdx > 0)
|
||||
{
|
||||
int centerFlag = pt3D.nPointIdx >> 4;
|
||||
if (centerFlag > 0)
|
||||
{
|
||||
rgb = { 180, 0, 0 };
|
||||
size = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
rgb = objColor[pt3D.nPointIdx % 8];
|
||||
size = 2;
|
||||
}
|
||||
}
|
||||
else //if (pt3D->nPointIdx == 0)
|
||||
{
|
||||
rgb = { 250, 250, 250 };
|
||||
size = 1;
|
||||
}
|
||||
double x = pt3D.pt3D.x;
|
||||
double y = pt3D.pt3D.y;
|
||||
int px = (int)((x - x_range.min) / scale + sideWidth);
|
||||
int py = (int)((y - y_range.min) / scale + sideWidth);
|
||||
if (size == 1)
|
||||
img.at<cv::Vec3b>(py, px) = cv::Vec3b(rgb[2], rgb[1], rgb[0]);
|
||||
else
|
||||
cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1);
|
||||
}
|
||||
}
|
||||
if (rodInfo.size() > 0)
|
||||
{
|
||||
|
||||
for (int i = 0; i < (int)rodInfo.size(); i++)
|
||||
{
|
||||
if (i == 0)
|
||||
rgb = { 255, 255, 0 };
|
||||
else
|
||||
rgb = { 255, 0, 0 };
|
||||
size = int(10.0 / scale);
|
||||
int px = (int)((rodInfo[i].center.x - x_range.min) / scale + sideWidth);
|
||||
int py = (int)((rodInfo[i].center.y - y_range.min) / scale + sideWidth);
|
||||
cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1);
|
||||
//画圆
|
||||
int px1 = (int)((rodInfo[i].startPt.x - x_range.min) / scale + sideWidth);
|
||||
int py1 = (int)((rodInfo[i].startPt.y - y_range.min) / scale + sideWidth);
|
||||
int px2 = (int)((rodInfo[i].endPt.x - x_range.min) / scale + sideWidth);
|
||||
int py2 = (int)((rodInfo[i].endPt.y - y_range.min) / scale + sideWidth);
|
||||
size = int(5.0 / scale);
|
||||
if (size == 0)
|
||||
size = 1;
|
||||
cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), size);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void _genXOYProjectionImage_rodInfo(
|
||||
cv::String& fileName,
|
||||
std::vector<std::vector< SVzNL3DPosition>>& scanLines,
|
||||
const double scale,
|
||||
std::vector<SSX_rodPositionInfo>& rodInfo,
|
||||
const double rpy[3], const double dirLen)
|
||||
{
|
||||
//旋转视角显示
|
||||
double matrix3d[9];
|
||||
EulerRpyToRotation1(rpy, matrix3d);
|
||||
|
||||
std::vector<SSX_rodPositionInfo> rotatedRod;
|
||||
for (int i = 0; i < (int)rodInfo.size(); i++)
|
||||
{
|
||||
SSX_rodPositionInfo rotateObj;
|
||||
rotateObj.center = _ptRotate(rodInfo[i].center, matrix3d);
|
||||
rotateObj.axialDir = _ptRotate(rodInfo[i].axialDir, matrix3d);
|
||||
rotateObj.normalDir = _ptRotate(rodInfo[i].normalDir, matrix3d);
|
||||
rotateObj.startPt = _ptRotate(rodInfo[i].startPt, matrix3d);
|
||||
rotateObj.endPt = _ptRotate(rodInfo[i].endPt, matrix3d);
|
||||
rotatedRod.push_back(rotateObj);
|
||||
}
|
||||
|
||||
std::vector<std::vector< SVzNL3DPosition>> rotateLines;
|
||||
SVzNLRangeD rx_range, ry_range;
|
||||
_rotateCloudPts(scanLines, matrix3d, rotateLines, rx_range, ry_range);
|
||||
|
||||
//统计X和Y的范围
|
||||
int x_cols = (int)((rx_range.max - rx_range.min) / scale) + 1;
|
||||
if (x_cols % 2 == 1)
|
||||
x_cols += 1;
|
||||
int y_rows = (int)((ry_range.max - ry_range.min) / scale) + 1;
|
||||
if (y_rows % 2 == 1)
|
||||
y_rows += 1;
|
||||
|
||||
int sideWidth = 32;
|
||||
int imgCols = x_cols + sideWidth * 2;
|
||||
int imgRows = y_rows + sideWidth * 2;
|
||||
|
||||
cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3);
|
||||
//计算投影比例
|
||||
_XOYprojection_rodInfo(img, rotateLines, rotatedRod, scale, sideWidth, rx_range, ry_range, dirLen);
|
||||
cv::imwrite(fileName, img);
|
||||
return;
|
||||
}
|
||||
|
||||
#define ROD_POSITION_TEST_GROUP 7
|
||||
#define TEST_CONVERT_TO_GRID 0
|
||||
#define TEST_COMPUTE_CALIB_PARA 0
|
||||
#define TEST_COMPUTE_ROD_POSITION 1
|
||||
@ -1421,17 +1640,18 @@ void rodPositionTest(void)
|
||||
"F:/ShangGu/项目/冠钦项目/胶布圆棒抓取/模拟测试数据2/", //4
|
||||
|
||||
"F:/ShangGu/项目/冠钦项目/矩森棒材抓取/异常点云/", //5
|
||||
"F:/ShangGu/项目/冠钦项目/矩森棒材抓取/异常点云2/", //6
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[ROD_POSITION_TEST_GROUP] = {
|
||||
{1,8}, {1,3}, {1,31},
|
||||
{1,5},{1,11},{1,26}
|
||||
{1,5},{1,11},{1,26},{1,41}
|
||||
};
|
||||
|
||||
const char* ver = wd_rodAndBarDetectionVersion();
|
||||
printf("ver:%s\n", ver);
|
||||
|
||||
for (int grp = 5; grp <= 5; grp++)
|
||||
for (int grp = 0; grp <= 6; grp++)
|
||||
{
|
||||
SSG_planeCalibPara poseCalibPara;
|
||||
//初始化成单位阵
|
||||
@ -1459,12 +1679,14 @@ void rodPositionTest(void)
|
||||
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx =16;
|
||||
//fidx =10;
|
||||
char _scan_file[256];
|
||||
if( (3 == grp) ||(4 == grp))
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_Hi229156.txt", dataPath[grp], fidx);
|
||||
else if(5 == grp)
|
||||
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
|
||||
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
|
||||
|
||||
@ -1487,6 +1709,11 @@ void rodPositionTest(void)
|
||||
rodParam.diameter = 96.0; //圆棒直径
|
||||
rodParam.len = 440;
|
||||
}
|
||||
else if (grp == 6)
|
||||
{
|
||||
rodParam.diameter = 62.0; //圆棒直径
|
||||
rodParam.len = 380;
|
||||
}
|
||||
else
|
||||
{
|
||||
rodParam.diameter = 68.0; //圆棒直径
|
||||
@ -1494,8 +1721,8 @@ void rodPositionTest(void)
|
||||
}
|
||||
|
||||
SSG_cornerParam cornerParam;
|
||||
cornerParam.cornerTh = 45; //45度角
|
||||
cornerParam.scale = rodParam.diameter / 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
||||
cornerParam.cornerTh = 30; //45度角
|
||||
cornerParam.scale = rodParam.diameter / 6; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
||||
cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4;
|
||||
cornerParam.minEndingGap_z = 5.0;
|
||||
cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平
|
||||
@ -1506,9 +1733,9 @@ void rodPositionTest(void)
|
||||
filterParam.outlierTh = 5;
|
||||
|
||||
SSG_treeGrowParam growParam;
|
||||
growParam.maxLineSkipNum = 5;
|
||||
growParam.yDeviation_max = 10.0;
|
||||
growParam.maxSkipDistance = 10.0;
|
||||
growParam.maxLineSkipNum = 15;
|
||||
growParam.yDeviation_max = 20.0;
|
||||
growParam.maxSkipDistance = 25.0;
|
||||
growParam.zDeviation_max = 10.0;//
|
||||
growParam.minLTypeTreeLen = rodParam.len * 0.75; //mm, 螺杆长度
|
||||
growParam.minVTypeTreeLen = rodParam.len * 0.75; //mm
|
||||
@ -1533,6 +1760,14 @@ void rodPositionTest(void)
|
||||
_outputRGBDScan_RGBD_rodInfo(_scan_file, scanLines, rodInfo);
|
||||
sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx);
|
||||
_outputRodtInfo(_scan_file, rodInfo);
|
||||
#if 1
|
||||
sprintf_s(_scan_file, "%sresult/%d_result_projection.png", dataPath[grp], fidx);
|
||||
double rpy[3] = { -60, 0, 10 }; //{ 0,-45, 0 }; //
|
||||
double angleDrawLen = 15;
|
||||
double displayScale = 1.0;
|
||||
cv::String imgName(_scan_file);
|
||||
_genXOYProjectionImage_rodInfo(imgName, scanLines, displayScale, rodInfo, rpy, angleDrawLen);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -265,6 +265,7 @@ SG_APISHARED_EXPORT void wd_getRodArcFeature_segmentPeakCornerMethod(
|
||||
const double minSegSize,
|
||||
const double peakChkWin,
|
||||
const SSG_cornerParam cornerPara,
|
||||
bool doSmoothing,
|
||||
std::vector<SWD_rodArcFeature>& line_rodArcs //»·
|
||||
);
|
||||
|
||||
|
||||
@ -849,7 +849,7 @@ void wd_getRodArcFeatureGrowingTrees(
|
||||
{
|
||||
for (int i = 0, i_max = (int)all_lineFeatures.size(); i < i_max; i++)
|
||||
{
|
||||
if (i == 630)
|
||||
if (i == 721)
|
||||
int kkk = 1;
|
||||
std::vector<SWD_rodArcFeature>& a_lineFeatures = all_lineFeatures[i];
|
||||
for (int j = 0, j_max = (int)a_lineFeatures.size(); j < j_max; j++)
|
||||
|
||||
@ -4319,28 +4319,43 @@ void _computeDirAngle_perSeg_2(
|
||||
int vPtIdxEnd = vPtIdxStart + segs[segIdx].len - 1;
|
||||
for (int i = vPtIdxStart; i <= vPtIdxEnd; i++)
|
||||
{
|
||||
if (i == 345)
|
||||
int kkk = 1;
|
||||
|
||||
if (vldPts[i].pt3D.z < 1e-4) //seg内空白点，用type区分
|
||||
{
|
||||
ptDirAngles[i].pntIdx = i;
|
||||
ptDirAngles[i].type = -1;
|
||||
continue;
|
||||
}
|
||||
//前向寻找
|
||||
int pre_i = -1;
|
||||
for (int j = i - 1; j >= vPtIdxStart; j--)
|
||||
{
|
||||
double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
|
||||
pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
|
||||
if (dist >= cornerPara.scale)
|
||||
if (vldPts[j].pt3D.z > 1e-4)
|
||||
{
|
||||
pre_i = j;
|
||||
break;
|
||||
double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
|
||||
pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
|
||||
if (dist >= cornerPara.scale)
|
||||
{
|
||||
pre_i = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
//后向寻找
|
||||
int post_i = -1;
|
||||
for (int j = i + 1; j <= vPtIdxEnd; j++)
|
||||
{
|
||||
double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
|
||||
pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
|
||||
if (dist >= cornerPara.scale)
|
||||
if (vldPts[j].pt3D.z > 1e-4)
|
||||
{
|
||||
post_i = j;
|
||||
break;
|
||||
double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
|
||||
pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
|
||||
if (dist >= cornerPara.scale)
|
||||
{
|
||||
post_i = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
//计算拐角
|
||||
@ -4704,6 +4719,7 @@ void wd_getRingArcFeature(
|
||||
return;
|
||||
}
|
||||
|
||||
#if 0
|
||||
//此处考虑到ZPeak会有波动，考虑到方向角计算是以尺度为基准，以方向角为准
|
||||
bool _chkRodArcFeature(
|
||||
int pkIdx, double searchWin,
|
||||
@ -4823,6 +4839,146 @@ bool _chkRodArcFeature(
|
||||
}
|
||||
return false;
|
||||
}
|
||||
#else
|
||||
//此处考虑到ZPeak会有波动，考虑到方向角计算是以尺度为基准，以方向角为准
|
||||
bool _chkRodArcFeature(
|
||||
int pkIdx, double searchWin,
|
||||
std::vector< SSG_pntDirAngle>& ptDirAngles,
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
const double arcPerPointCornerMinValue, //arc上每个点的转角最小值
|
||||
const double arcTotalCornerMinValue, //整个Arc的转角最小值
|
||||
SSG_intPair& a_rodPos)
|
||||
{
|
||||
//pkIdx可能会有波动
|
||||
//寻找前向角和后向角最接近的点
|
||||
double realPkCorner = 0;
|
||||
double realPkRatio = -1;
|
||||
int realPkIdx = pkIdx;
|
||||
//向前搜索
|
||||
for (int i = pkIdx; i >= 0; i--)
|
||||
{
|
||||
if (lineData[i].pt3D.z < 1e-4)
|
||||
continue;
|
||||
|
||||
if (ptDirAngles[i].type < 0)
|
||||
continue;
|
||||
|
||||
if (ptDirAngles[i].pntIdx < 0)
|
||||
break;
|
||||
|
||||
double dist = sqrt(pow(lineData[pkIdx].pt3D.y - lineData[i].pt3D.y, 2) + pow(lineData[pkIdx].pt3D.z - lineData[i].pt3D.z, 2));
|
||||
if (dist > searchWin)
|
||||
break;
|
||||
if ((ptDirAngles[i].forwardAngle > 0) && (ptDirAngles[i].backwardAngle < 0)&& (ptDirAngles[i].corner < -arcPerPointCornerMinValue))
|
||||
{
|
||||
double ratio = abs(ptDirAngles[i].forwardAngle) < abs(ptDirAngles[i].backwardAngle) ?
|
||||
(abs(ptDirAngles[i].forwardAngle) / abs(ptDirAngles[i].backwardAngle)) :
|
||||
(abs(ptDirAngles[i].backwardAngle) / abs(ptDirAngles[i].forwardAngle));
|
||||
|
||||
if (realPkRatio < 0)
|
||||
{
|
||||
realPkRatio = ratio;
|
||||
realPkIdx = i;
|
||||
realPkCorner = abs(ptDirAngles[i].corner);
|
||||
}
|
||||
else if (realPkRatio < ratio)
|
||||
{
|
||||
realPkRatio = ratio;
|
||||
realPkIdx = i;
|
||||
realPkCorner = abs(ptDirAngles[i].corner);
|
||||
}
|
||||
}
|
||||
}
|
||||
//向后搜索
|
||||
for (int i = pkIdx + 1; i < (int)lineData.size(); i++)
|
||||
{
|
||||
if (lineData[i].pt3D.z < 1e-4)
|
||||
continue;
|
||||
|
||||
if (ptDirAngles[i].type < 0) //seg内空白点，跳过
|
||||
continue;
|
||||
|
||||
if (ptDirAngles[i].pntIdx < 0) //seg端点，结束
|
||||
break;
|
||||
double dist = sqrt(pow(lineData[pkIdx].pt3D.y - lineData[i].pt3D.y, 2) + pow(lineData[pkIdx].pt3D.z - lineData[i].pt3D.z, 2));
|
||||
if (dist > searchWin)
|
||||
break;
|
||||
|
||||
if ((ptDirAngles[i].forwardAngle > 0) && (ptDirAngles[i].backwardAngle < 0) && (ptDirAngles[i].corner < -arcPerPointCornerMinValue))
|
||||
{
|
||||
double ratio = abs(ptDirAngles[i].forwardAngle) < abs(ptDirAngles[i].backwardAngle) ?
|
||||
(abs(ptDirAngles[i].forwardAngle) / abs(ptDirAngles[i].backwardAngle)) :
|
||||
(abs(ptDirAngles[i].backwardAngle) / abs(ptDirAngles[i].forwardAngle));
|
||||
|
||||
if (realPkRatio < 0)
|
||||
{
|
||||
realPkRatio = ratio;
|
||||
realPkIdx = i;
|
||||
realPkCorner = abs(ptDirAngles[i].corner);
|
||||
}
|
||||
else if (realPkRatio < ratio)
|
||||
{
|
||||
realPkRatio = ratio;
|
||||
realPkIdx = i;
|
||||
realPkCorner = abs(ptDirAngles[i].corner);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (realPkRatio < 0)
|
||||
return false;
|
||||
|
||||
//向后搜索
|
||||
int searchIdx = realPkIdx;
|
||||
int pre_idx = realPkIdx;
|
||||
while (1)
|
||||
{
|
||||
if ((ptDirAngles[searchIdx].backwardAngle > -1e-4) || (ptDirAngles[searchIdx].pntIdx < 0)) //isEnding
|
||||
{
|
||||
for (int m = searchIdx; m <= realPkIdx; m++)
|
||||
{
|
||||
if ((ptDirAngles[m].type >= 0) && (ptDirAngles[m].backwardAngle < -1e-4))
|
||||
{
|
||||
pre_idx = m;
|
||||
break;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
pre_idx = ptDirAngles[searchIdx].backwardPntIdx;
|
||||
searchIdx = ptDirAngles[searchIdx].backwardPntIdx;
|
||||
}
|
||||
//向前搜索
|
||||
searchIdx = realPkIdx;
|
||||
int post_idx = realPkIdx;
|
||||
while (1)
|
||||
{
|
||||
if ((ptDirAngles[searchIdx].forwardAngle < 1e-4) || (ptDirAngles[searchIdx].pntIdx < 0)) //isEnding
|
||||
{
|
||||
for (int m = searchIdx; m >= realPkIdx; m--)
|
||||
{
|
||||
if ((ptDirAngles[m].type >= 0) && (ptDirAngles[m].forwardAngle > 1e-4))
|
||||
{
|
||||
post_idx = m;
|
||||
break;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
post_idx = ptDirAngles[searchIdx].forwardPntIdx;
|
||||
searchIdx = ptDirAngles[searchIdx].forwardPntIdx;
|
||||
}
|
||||
|
||||
double totalCorner = ptDirAngles[post_idx].forwardAngle - ptDirAngles[pre_idx].backwardAngle;
|
||||
if (totalCorner > arcTotalCornerMinValue) //整个圆弧需要至少60度的扇区。
|
||||
{
|
||||
a_rodPos.idx = realPkIdx;
|
||||
a_rodPos.data_0 = pre_idx;
|
||||
a_rodPos.data_1 = post_idx;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
/// <summary>
|
||||
/// 提取激光线上的圆环的上半段弧。
|
||||
@ -4846,7 +5002,7 @@ void wd_getRodArcFeature_peakCornerMethod(
|
||||
)
|
||||
{
|
||||
double arcPerPointCornerMinValue = 5; //arc上每个点的转角最小值
|
||||
double arcTotalCornerMinValue = 30; //整个Arc的转角最小值
|
||||
double arcTotalCornerMinValue = 45; //整个Arc的转角最小值
|
||||
//计算前向角和后向角
|
||||
std::vector< SSG_pntDirAngle> ptDirAngles;
|
||||
wd_computeDirAngle_wholeLine( lineData, cornerPara, ptDirAngles);
|
||||
@ -5222,11 +5378,11 @@ void wd_getRodArcFeature_segmentPeakCornerMethod(
|
||||
const double minSegSize,
|
||||
const double peakChkWin,
|
||||
const SSG_cornerParam cornerPara,
|
||||
bool doSmoothing,
|
||||
std::vector<SWD_rodArcFeature>& line_rodArcs //环
|
||||
)
|
||||
{
|
||||
double arcPerPointCornerMinValue = 5.0; //arc上每个点的转角最小值
|
||||
double arcTotalCornerMinValue = 30; //整个Arc的转角最小值
|
||||
double arcPerPointCornerMinValue = 3.0;// 5.0; //arc上每个点的转角最小值
|
||||
|
||||
std::vector<SSG_RUN> segs;
|
||||
wd_lineDataSegment_dist_2(
|
||||
@ -5235,6 +5391,23 @@ void wd_getRodArcFeature_segmentPeakCornerMethod(
|
||||
maxDistTh,
|
||||
minSegSize
|
||||
);
|
||||
|
||||
if (true == doSmoothing)
|
||||
{
|
||||
int smoothWin = 3;
|
||||
int segSize = (int)segs.size();
|
||||
for (int segIdx = 0; segIdx < segSize; segIdx++)
|
||||
{
|
||||
int vPtIdxStart = segs[segIdx].start;
|
||||
int vPtIdxEnd = vPtIdxStart + segs[segIdx].len - 1;
|
||||
std::vector<SVzNL3DPosition> segData;
|
||||
segData.insert(segData.end(), lineData.begin() + vPtIdxStart, lineData.begin() + vPtIdxEnd + 1);
|
||||
std::vector<SVzNL3DPosition> segSmoothData;
|
||||
sg_lineDataSmoothing(segData, smoothWin, segSmoothData);
|
||||
std::copy(segSmoothData.begin(), segSmoothData.end(), lineData.begin() + vPtIdxStart);
|
||||
}
|
||||
}
|
||||
|
||||
//计算前向角和后向角
|
||||
std::vector< SSG_pntDirAngle> ptDirAngles;
|
||||
_computeDirAngle_perSeg_2(lineData, segs, cornerPara, ptDirAngles);
|
||||
@ -5256,7 +5429,7 @@ void wd_getRodArcFeature_segmentPeakCornerMethod(
|
||||
bool isArc = _chkRodArcFeature(
|
||||
pkIdx, cornerPara.scale,
|
||||
ptDirAngles, lineData,
|
||||
arcPerPointCornerMinValue, arcTotalCornerMinValue,
|
||||
arcPerPointCornerMinValue, cornerPara.cornerTh,
|
||||
a_rodPos);
|
||||
if ( (true == isArc) && (prePos.idx != a_rodPos.idx))
|
||||
{
|
||||
|
||||
@ -30,7 +30,8 @@
|
||||
//version 1.3.7 : 新的定位盘中心测量功能：进一步优化了内部参数，优化了垂直点去除效果
|
||||
//version 1.3.8 : 新的螺杆定位算法，使用PCA方法确定螺杆轴向
|
||||
//version 1.4.0 : 矩森棒材抓取算法改进：（1）修正了两个棒材被识别成一根的问题（2）增强了对噪点的容忍度，增强了鲁棒性
|
||||
std::string m_strVersion = "RodAndBarDetection_1.4.0";
|
||||
//version 1.5.0 : 矩森棒材抓取算法改进：加强了对强噪声点云的处理
|
||||
std::string m_strVersion = "RodAndBarDetection_1.5.0";
|
||||
const char* wd_rodAndBarDetectionVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -249,18 +250,20 @@ void rodArcFeatueDetection(
|
||||
const double segment_maxDistTh,
|
||||
const double segment_minSegSze,
|
||||
const double rodDiameter,
|
||||
bool doSmoothing, //对于水平扫描数据，波动比较大，需要平滑处理
|
||||
std::vector<std::vector<SWD_rodArcFeature>>& arcFeatures)
|
||||
{
|
||||
int lineNum = (int)scanLines.size();
|
||||
int linePtNum = (int)scanLines[0].size();
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
if (line == 780)
|
||||
if (line == 810)
|
||||
int kkk = 1;
|
||||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||||
|
||||
// Filter outliers from line data
|
||||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||||
|
||||
// Extract rod arc features
|
||||
std::vector<SWD_rodArcFeature> line_rodArcs;
|
||||
#if 0
|
||||
@ -283,6 +286,7 @@ void rodArcFeatueDetection(
|
||||
segment_minSegSze,
|
||||
rodDiameter / 2,
|
||||
cornerPara,
|
||||
doSmoothing,
|
||||
line_rodArcs //环
|
||||
);
|
||||
#endif
|
||||
@ -533,6 +537,7 @@ SVzNL3DRangeD _getPointCloudROI(std::vector<SWD3DPointPostion>& scanData)
|
||||
segment_maxDistTh,
|
||||
segment_minSegSize,
|
||||
rodDiameter,
|
||||
false,
|
||||
arcFeatures);
|
||||
|
||||
//特征生长
|
||||
@ -3437,6 +3442,131 @@ void sx_rodPositioning(
|
||||
hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
||||
hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
||||
}
|
||||
}
|
||||
|
||||
//去除垂直段
|
||||
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(
|
||||
hLines_raw[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;
|
||||
hLines_raw[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(
|
||||
hLines_raw[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;
|
||||
hLines_raw[j][line].pt3D.z = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//内部参数
|
||||
@ -3444,14 +3574,14 @@ void sx_rodPositioning(
|
||||
const double segment_minSegSize = rodParam.diameter / 8;
|
||||
//在垂直方向上分别提取ARC特征，并进行特征生长
|
||||
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_v;
|
||||
rodArcFeatueDetection( scanLines, cornerPara, filterParam, segment_maxDistTh, segment_minSegSize, rodParam.diameter, arcFeatures_v);
|
||||
rodArcFeatueDetection( scanLines, cornerPara, filterParam, segment_maxDistTh, segment_minSegSize, rodParam.diameter, false, arcFeatures_v);
|
||||
//特征生长
|
||||
std::vector<SWD_rodArcFeatureTree> rodArcTrees_v;
|
||||
wd_getRodArcFeatureGrowingTrees(arcFeatures_v, rodArcTrees_v, growParam);
|
||||
|
||||
//水平方向
|
||||
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_h;
|
||||
rodArcFeatueDetection(hLines_raw, cornerPara, filterParam, segment_maxDistTh, segment_minSegSize, rodParam.diameter, arcFeatures_h);
|
||||
rodArcFeatueDetection(hLines_raw, cornerPara, filterParam, segment_maxDistTh, segment_minSegSize, rodParam.diameter, true, arcFeatures_h);
|
||||
//特征生长
|
||||
std::vector<SWD_rodArcFeatureTree> rodArcTrees_h;
|
||||
wd_getRodArcFeatureGrowingTrees(arcFeatures_h, rodArcTrees_h, growParam);
|
||||
@ -3462,7 +3592,6 @@ void sx_rodPositioning(
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
int objNum_v = (int)rodArcTrees_v.size();
|
||||
int objNum_h = (int)rodArcTrees_h.size();
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
@ -4848,7 +4977,7 @@ void sx_rebarWeldSeamPositioning(
|
||||
const double segment_minSegSize = rodParam.diameter / 8;
|
||||
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_v;
|
||||
#if 1
|
||||
rodArcFeatueDetection(scanLines, cornerPara, filterParam, segment_maxDistTh, segment_minSegSize, rodParam.diameter, arcFeatures_v);
|
||||
rodArcFeatueDetection(scanLines, cornerPara, filterParam, segment_maxDistTh, segment_minSegSize, rodParam.diameter, false, arcFeatures_v);
|
||||
#else
|
||||
lineArcAndWeldFeatueDetection(scanLines, cornerPara, filterParam, rodParam.diameter, arcFeatures_v);
|
||||
#endif
|
||||
@ -4875,7 +5004,7 @@ void sx_rebarWeldSeamPositioning(
|
||||
}
|
||||
//水平方向
|
||||
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_h;
|
||||
rodArcFeatueDetection(hLines_raw, cornerPara, filterParam, segment_maxDistTh, segment_minSegSize, rodParam.diameter, arcFeatures_h);
|
||||
rodArcFeatueDetection(hLines_raw, cornerPara, filterParam, segment_maxDistTh, segment_minSegSize, rodParam.diameter, true, arcFeatures_h);
|
||||
//特征生长
|
||||
std::vector<SWD_rodArcFeatureTree> allRodArcTrees_h;
|
||||
wd_getRodArcFeatureGrowingTrees(arcFeatures_h, allRodArcTrees_h, growParam);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user