搭接焊缝检测算法提交
This commit is contained in:
parent
f855ffd70b
commit
94e8a1efe8
@ -149,7 +149,8 @@ void _outputScanDataFile_vector_h(char* fileName, std::vector<std::vector<SVzNL3
|
||||
void _outputRGBDScanLapWeld_RGBD(
|
||||
char* fileName,
|
||||
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
|
||||
std::vector< std::vector<SVzNL3DPoint>> weldOPs)
|
||||
std::vector< std::vector<SVzNL3DPoint>> weldOPs,
|
||||
bool outDebugInfo)
|
||||
{
|
||||
int lineNum = (int)scanLines.size();
|
||||
std::ofstream sw(fileName);
|
||||
@ -191,18 +192,38 @@ void _outputRGBDScanLapWeld_RGBD(
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
SVzNL3DPosition* pt3D = &scanLines[line][i];
|
||||
if (pt3D->nPointIdx > 0)
|
||||
int kkk = 1;
|
||||
int featureType_v = pt3D->nPointIdx & 0xffff;
|
||||
int featureType_h = featureType_v >> 4;
|
||||
featureType_v &= 0xff;
|
||||
if (LINE_FEATURE_PEAK_TOP == featureType_v)
|
||||
if (true == outDebugInfo)
|
||||
{
|
||||
rgb = { 255, 97, 0 };
|
||||
size = 5;
|
||||
}
|
||||
else if (LINE_FEATURE_PEAK_TOP == featureType_h)
|
||||
{
|
||||
rgb = { 97, 255, 0 };
|
||||
size = 5;
|
||||
if (LINE_FEATURE_L_JUMP_H2L == featureType_v)
|
||||
{
|
||||
rgb = { 255, 97, 0 };
|
||||
size = 5;
|
||||
}
|
||||
else if (LINE_FEATURE_L_JUMP_L2H == featureType_v)
|
||||
{
|
||||
rgb = objColor[7];
|
||||
size = 5;
|
||||
}
|
||||
else if (LINE_FEATURE_L_JUMP_H2L == featureType_h)
|
||||
{
|
||||
rgb = objColor[6];
|
||||
size = 5;
|
||||
}
|
||||
else if (LINE_FEATURE_L_JUMP_L2H == featureType_h)
|
||||
{
|
||||
rgb = { 97, 255, 0 };
|
||||
size = 5;
|
||||
}
|
||||
else
|
||||
{
|
||||
rgb = { 200, 200, 200 };
|
||||
size = 1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -225,11 +246,10 @@ void _outputRGBDScanLapWeld_RGBD(
|
||||
linePtNum += (int)weldOPs[i].size();
|
||||
sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl;
|
||||
|
||||
rgb = { 0, 0, 255 };
|
||||
rgb = { 255, 0, 0 };
|
||||
size = 25;
|
||||
for (int i = 0; i < weldNum; i++)
|
||||
{
|
||||
rgb = objColor[i % 8];
|
||||
for (int j = 0; j < (int)weldOPs[i].size(); j++)
|
||||
{
|
||||
float x = (float)weldOPs[i][j].x;
|
||||
@ -241,13 +261,37 @@ void _outputRGBDScanLapWeld_RGBD(
|
||||
}
|
||||
}
|
||||
//加一个点，用于跳过显示工具bug
|
||||
rgb = objColor[0];
|
||||
float x = (float)weldOPs[0][0].x;
|
||||
float y = (float)weldOPs[0][0].y;
|
||||
float z = (float)weldOPs[0][0].z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
|
||||
double exLen = 5.0;
|
||||
rgb = objColor[0];
|
||||
size = 3;
|
||||
for (int i = 0; i < weldNum; i++)
|
||||
{
|
||||
SVzNL3DPoint pt0 = weldOPs[i][0];
|
||||
SVzNL3DPoint pt1 = weldOPs[i].back();
|
||||
double len = sqrt(pow(pt0.x - pt1.x, 2) + pow(pt0.y - pt1.y, 2));
|
||||
double k = exLen / len;
|
||||
sw << "Poly_" << i << "_2" << std::endl;
|
||||
double x = -k * (pt1.x - pt0.x) + pt0.x;
|
||||
double y = -k * (pt1.y - pt0.y) + pt0.y;
|
||||
double z = -k * (pt1.z - pt0.z) + pt0.z;
|
||||
sw << "{" << (float)x << "," << (float)y << "," << (float)z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
|
||||
x = -k * (pt0.x - pt1.x) + pt1.x;
|
||||
y = -k * (pt0.y - pt1.y) + pt1.y;
|
||||
z = -k * (pt0.z - pt1.z) + pt1.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
}
|
||||
}
|
||||
sw.close();
|
||||
}
|
||||
@ -655,7 +699,7 @@ void _convertToGridData(std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
|
||||
#define CONVERT_TO_GRID 0
|
||||
#define TEST_COMPUTE_CALIB_PARA 0
|
||||
#define TEST_COMPUTE_GLOVE_POSITION 1
|
||||
#define TEST_COMPUTE_POSITION 1
|
||||
#define TEST_GROUP 1
|
||||
int main()
|
||||
{
|
||||
@ -665,7 +709,7 @@ int main()
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
{0,6}
|
||||
{1,6}
|
||||
};
|
||||
|
||||
#if CONVERT_TO_GRID
|
||||
@ -720,7 +764,7 @@ int main()
|
||||
int kkk = 1;
|
||||
//行处理
|
||||
//调平，去除地面
|
||||
sx_lineDataR(scanData[i], calibPara.planeCalib, -1);// calibPara.planeHeight);
|
||||
sx_lineDataR(scanData[i], calibPara.planeCalib, -1); // calibPara.planeHeight);
|
||||
}
|
||||
//
|
||||
char calibFile[250];
|
||||
@ -734,7 +778,7 @@ int main()
|
||||
}
|
||||
#endif
|
||||
|
||||
#if TEST_COMPUTE_GLOVE_POSITION
|
||||
#if TEST_COMPUTE_POSITION
|
||||
for (int grp = 0; grp <= 0; grp++)
|
||||
{
|
||||
SSG_planeCalibPara poseCalibPara;
|
||||
@ -771,7 +815,7 @@ int main()
|
||||
int kkk = 1;
|
||||
//行处理
|
||||
//调平，去除地面
|
||||
sx_lineDataR(scanLines[i], poseCalibPara.planeCalib, -1);// calibPara.planeHeight);
|
||||
sx_lineDataR(scanLines[i], poseCalibPara.planeCalib, -1);
|
||||
}
|
||||
#if 0
|
||||
char _out_file[256];
|
||||
@ -779,24 +823,26 @@ int main()
|
||||
int headNullLines = 0;
|
||||
_outputScanDataFile_vector(_out_file, scanLines, false, &headNullLines);
|
||||
#endif
|
||||
SSG_treeGrowParam growParam;
|
||||
growParam.maxLineSkipNum = 5;
|
||||
growParam.yDeviation_max = 1.0;
|
||||
growParam.maxSkipDistance = 5.0;
|
||||
growParam.zDeviation_max = 2;// algoParam.bagParam.bagH / 2; //袋子高度1/2
|
||||
growParam.minLTypeTreeLen = 30.0; //mm
|
||||
growParam.minVTypeTreeLen = 30.0; //mm
|
||||
SSG_cornerParam cornerParam;
|
||||
cornerParam.cornerTh =30; //45度角
|
||||
cornerParam.scale = 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
||||
cornerParam.minEndingGap = 20;// algoParam.bagParam.bagW / 4;
|
||||
cornerParam.minEndingGap_z = 20; // algoParam.bagParam.bagH / 4;
|
||||
cornerParam.jumpCornerTh_1 = 10;
|
||||
cornerParam.jumpCornerTh_2 = 30;
|
||||
|
||||
SSX_lapWeldParam lapWeldParam;
|
||||
lapWeldParam.lapHeight = 2.0;
|
||||
lapWeldParam.weldMinLen = 2.0;
|
||||
lapWeldParam.lapHeight = 1.5;
|
||||
lapWeldParam.weldMinLen = 80.0;
|
||||
lapWeldParam.weldRefPoints = 2;
|
||||
lapWeldParam.scanMode = keSX_ScanMode_V;
|
||||
SSG_cornerParam cornerParam;
|
||||
cornerParam.cornerTh = 25; //45度角
|
||||
cornerParam.scale = 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
||||
cornerParam.minEndingGap = 6;// algoParam.bagParam.bagW / 4;
|
||||
cornerParam.minEndingGap_z = lapWeldParam.lapHeight; // algoParam.bagParam.bagH / 4;
|
||||
cornerParam.jumpCornerTh_1 = 10;
|
||||
cornerParam.jumpCornerTh_2 = 25;
|
||||
SSG_treeGrowParam growParam;
|
||||
growParam.maxLineSkipNum = 10;
|
||||
growParam.yDeviation_max = 3.0;
|
||||
growParam.maxSkipDistance = 5.0;
|
||||
growParam.zDeviation_max = 1.0;// algoParam.bagParam.bagH / 2; //袋子高度1/2
|
||||
growParam.minLTypeTreeLen = lapWeldParam.weldMinLen; //mm
|
||||
growParam.minVTypeTreeLen = lapWeldParam.weldMinLen; //mm
|
||||
int errCode = 0;
|
||||
std::vector<std::vector<SVzNL3DPoint>> weldOps;
|
||||
sx_getLapWeldPostion(
|
||||
@ -804,6 +850,7 @@ int main()
|
||||
cornerParam,
|
||||
growParam,
|
||||
lapWeldParam,
|
||||
poseCalibPara,
|
||||
weldOps,
|
||||
&errCode);
|
||||
|
||||
@ -811,7 +858,7 @@ int main()
|
||||
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
|
||||
//输出测试结果
|
||||
sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
|
||||
_outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, weldOps);
|
||||
_outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, weldOps, false);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -74,7 +74,7 @@ SG_APISHARED_EXPORT void sg_getLineJumpFeature_cornerMethod(
|
||||
int dataSize,
|
||||
int lineIdx,
|
||||
const SSG_cornerParam cornerPara, //scale通常取bagH的1/4
|
||||
std::vector< SSG_basicFeature1D> jumpFeatures);
|
||||
std::vector< SSG_basicFeature1D>& jumpFeatures);
|
||||
|
||||
SG_APISHARED_EXPORT void wd_getLineGloveArcs(
|
||||
std::vector<SVzNL3DPosition>& lineData,
|
||||
|
||||
@ -1507,6 +1507,8 @@ SSG_planeCalibPara sg_getPlaneCalibPara2(
|
||||
//数据进行转换
|
||||
SVzNLRangeD calibZRange = { 0, -1 };
|
||||
topZRange = { 0, -1 };
|
||||
double sumMeanZ = 0;
|
||||
int sumSize = 0;
|
||||
for (int i = 0, i_max = (int)Points3ds.size(); i < i_max; i++)
|
||||
{
|
||||
//z
|
||||
@ -1531,6 +1533,8 @@ SSG_planeCalibPara sg_getPlaneCalibPara2(
|
||||
{
|
||||
calibZRange.min = a_calibPt.z;
|
||||
calibZRange.max = a_calibPt.z;
|
||||
sumMeanZ += a_calibPt.z;
|
||||
sumSize++;
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -1538,9 +1542,13 @@ SSG_planeCalibPara sg_getPlaneCalibPara2(
|
||||
calibZRange.min = a_calibPt.z;
|
||||
if (calibZRange.max < a_calibPt.z)
|
||||
calibZRange.max = a_calibPt.z;
|
||||
sumMeanZ += a_calibPt.z;
|
||||
sumSize++;
|
||||
}
|
||||
}
|
||||
planePara.planeHeight = calibZRange.min;
|
||||
if (sumSize > 0)
|
||||
sumMeanZ = sumMeanZ / (double)sumSize;
|
||||
planePara.planeHeight = sumMeanZ; // calibZRange.min;
|
||||
|
||||
return planePara;
|
||||
}
|
||||
|
||||
@ -1455,7 +1455,7 @@ void sg_getLineJumpFeature_cornerMethod(
|
||||
int dataSize,
|
||||
int lineIdx,
|
||||
const SSG_cornerParam cornerPara, //scale通常取bagH的1/4
|
||||
std::vector< SSG_basicFeature1D> jumpFeatures)
|
||||
std::vector< SSG_basicFeature1D>& jumpFeatures)
|
||||
{
|
||||
//去除零点
|
||||
std::vector< SVzNL3DPosition> vldPts;
|
||||
@ -1775,13 +1775,14 @@ void sg_getLineJumpFeature_cornerMethod(
|
||||
int pntIdx_2 = cornerFeatures[i+1].pntIdx;
|
||||
//计算距离和跳变高度
|
||||
double dist = abs(vldPts[pntIdx_1].pt3D.y - vldPts[pntIdx_2].pt3D.y);
|
||||
double height = cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z;
|
||||
if ((dist < cornerPara.minEndingGap) && (height < -cornerPara.minEndingGap_z))
|
||||
double height = abs(cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z);
|
||||
if ((dist < cornerPara.minEndingGap) && (height > cornerPara.minEndingGap_z))
|
||||
{
|
||||
SSG_basicFeature1D a_feature;
|
||||
a_feature.featureType = LINE_FEATURE_L_JUMP_L2H;
|
||||
a_feature.jumpPos = vldPts[pntIdx_1].pt3D;
|
||||
a_feature.jumpPos2D = { lineIdx, pntIdx_1 };
|
||||
a_feature.jumpPos2D = { lineIdx, vldPts[pntIdx_1].nPointIdx };
|
||||
a_feature.featureValue = cornerFeatures[i].backward_z;
|
||||
jumpFeatures.push_back(a_feature);
|
||||
}
|
||||
}
|
||||
@ -1794,13 +1795,14 @@ void sg_getLineJumpFeature_cornerMethod(
|
||||
int pntIdx_2 = cornerFeatures[i + 1].pntIdx;
|
||||
//计算距离和跳变高度
|
||||
double dist = abs(vldPts[pntIdx_1].pt3D.y - vldPts[pntIdx_2].pt3D.y);
|
||||
double height = cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z;
|
||||
double height = abs(cornerFeatures[i + 1].forward_z - cornerFeatures[i].backward_z);
|
||||
if ((dist < cornerPara.minEndingGap) && (height > cornerPara.minEndingGap_z))
|
||||
{
|
||||
SSG_basicFeature1D a_feature;
|
||||
a_feature.featureType = LINE_FEATURE_L_JUMP_H2L;
|
||||
a_feature.jumpPos = vldPts[pntIdx_2].pt3D;
|
||||
a_feature.jumpPos2D = { lineIdx, pntIdx_2 };
|
||||
a_feature.jumpPos2D = { lineIdx, vldPts[pntIdx_2].nPointIdx };
|
||||
a_feature.featureValue = cornerFeatures[i + 1].forward_z;
|
||||
jumpFeatures.push_back(a_feature);
|
||||
}
|
||||
}
|
||||
|
||||
@ -23,12 +23,78 @@ void sx_lineDataR(
|
||||
lineDataRT_vector(a_line, camPoseR, groundH);
|
||||
}
|
||||
|
||||
SVzNL2DPoint getFootPoint(double x0, double y0, double k, double b)
|
||||
{
|
||||
double A = k;
|
||||
double B = -1;
|
||||
double C = b;
|
||||
SVzNL2DPoint foot;
|
||||
foot.x = (B * B * x0 - A * B * y0 - A * C) / (A * A + B * B);
|
||||
foot.y = (-A * B * x0 + A * A * y0 - B * C) / (A * A + B * B);
|
||||
return foot;
|
||||
}
|
||||
|
||||
double getWinMeanZ(std::vector<SVzNL3DPoint>& points, int startIdx, int win)
|
||||
{
|
||||
int size = (int)points.size();
|
||||
double sumZ = 0;
|
||||
double sumSize = 0;
|
||||
for (int i = 0; i < win; i++)
|
||||
{
|
||||
int idx = i + startIdx;
|
||||
if (idx < size)
|
||||
{
|
||||
if (points[idx].z > 1e-4)
|
||||
{
|
||||
sumZ += points[idx].z;
|
||||
sumSize++;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (sumSize > 0)
|
||||
sumZ = sumZ / (double)sumSize;
|
||||
return sumZ;
|
||||
}
|
||||
//从焊缝的扫描点获取焊缝的起点和终点信息
|
||||
void getWeldPoint(std::vector<SVzNL3DPoint>& a_weld_contour, int midPtNum, std::vector<SVzNL3DPoint>& a_weld)
|
||||
{
|
||||
if (a_weld_contour.size() == 0)
|
||||
return;
|
||||
|
||||
//拟合直线
|
||||
double k, b;
|
||||
lineFitting(a_weld_contour, &k, &b);
|
||||
//寻找起点和终点
|
||||
SVzNL3DPoint startPt = a_weld_contour[0];
|
||||
SVzNL3DPoint endPt = a_weld_contour.back();
|
||||
SVzNL2DPoint foot_s = getFootPoint(startPt.x, startPt.y, k, b);
|
||||
SVzNL2DPoint foot_e = getFootPoint(endPt.x, endPt.y, k, b);
|
||||
double meanZ_s = getWinMeanZ(a_weld_contour, 0, 5);
|
||||
double meanZ_e = getWinMeanZ(a_weld_contour, (int)a_weld_contour.size()-1-5, 5);
|
||||
|
||||
SVzNL3DPoint pt_0 = { foot_s.x, foot_s.y, meanZ_s };
|
||||
SVzNL3DPoint pt_1 = { foot_e.x, foot_e.y, meanZ_e };
|
||||
a_weld.push_back(pt_0);
|
||||
double ratio = 1.0 / ((double)midPtNum + 1.0);
|
||||
|
||||
for (int i = 1; i <= midPtNum; i++)
|
||||
{
|
||||
SVzNL3DPoint a_pt;
|
||||
a_pt.x = (double)i * ratio * (pt_1.x - pt_0.x) + pt_0.x;
|
||||
a_pt.y = (double)i * ratio * (pt_1.y - pt_0.y) + pt_0.y;
|
||||
a_pt.z = (double)i * ratio * (pt_1.z - pt_0.z) + pt_0.z;
|
||||
a_weld.push_back(a_pt);
|
||||
}
|
||||
a_weld.push_back(pt_1);
|
||||
}
|
||||
|
||||
//提取搭接焊缝
|
||||
void sx_getLapWeldPostion(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SSG_cornerParam cornerPara,
|
||||
SSG_treeGrowParam growParam,
|
||||
SSX_lapWeldParam lapWeldParam,
|
||||
SSG_planeCalibPara groundCalibPara,
|
||||
std::vector<std::vector<SVzNL3DPoint>>& objOps,
|
||||
int* errCode)
|
||||
{
|
||||
@ -42,26 +108,39 @@ void sx_getLapWeldPostion(
|
||||
|
||||
int linePtNum = (int)scanLines[0].size();
|
||||
bool isGridData = true;
|
||||
//垂直arc特征提取
|
||||
//垂直跳变特征提取
|
||||
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
if ((keSX_ScanMode_V == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode))
|
||||
{
|
||||
if (line == 400)
|
||||
int kkk = 1;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
if (line == 400)
|
||||
int kkk = 1;
|
||||
|
||||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||||
if (linePtNum != (int)lineData.size())
|
||||
isGridData = false;
|
||||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||||
if (linePtNum != (int)lineData.size())
|
||||
isGridData = false;
|
||||
|
||||
std::vector<SSG_basicFeature1D> a_line_features;
|
||||
int dataSize = (int)lineData.size();
|
||||
sg_getLineJumpFeature_cornerMethod(
|
||||
&scanLines[line][0],
|
||||
dataSize,
|
||||
line,
|
||||
cornerPara, //scale通常取bagH的1/4
|
||||
a_line_features);
|
||||
jumpFeatures_v.push_back(a_line_features);
|
||||
std::vector<SSG_basicFeature1D> a_line_features;
|
||||
int dataSize = (int)lineData.size();
|
||||
sg_getLineJumpFeature_cornerMethod(
|
||||
&scanLines[line][0],
|
||||
dataSize,
|
||||
line,
|
||||
cornerPara, //scale通常取bagH的1/4
|
||||
a_line_features);
|
||||
//滤除地面
|
||||
std::vector<SSG_basicFeature1D> vld_features;
|
||||
for (int i = 0, i_max = a_line_features.size(); i < i_max; i++)
|
||||
{
|
||||
//将与地面形成的跳变去除
|
||||
if (a_line_features[i].featureValue < (groundCalibPara.planeHeight - 0.25))
|
||||
{
|
||||
vld_features.push_back(a_line_features[i]);
|
||||
}
|
||||
}
|
||||
jumpFeatures_v.push_back(vld_features);
|
||||
}
|
||||
}
|
||||
|
||||
if (false == isGridData)//数据不是网格格式
|
||||
@ -88,55 +167,77 @@ void sx_getLapWeldPostion(
|
||||
//水平arc特征提取
|
||||
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h;
|
||||
int lineNum_h = (int)hLines.size();
|
||||
for (int line = 0; line < lineNum_h; line++)
|
||||
if ((keSX_ScanMode_H == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode))
|
||||
{
|
||||
std::vector<SVzNL3DPosition>& lineData = hLines[line];
|
||||
for (int line = 0; line < lineNum_h; line++)
|
||||
{
|
||||
std::vector<SVzNL3DPosition>& lineData = hLines[line];
|
||||
|
||||
std::vector<SSG_basicFeature1D> a_line_features;
|
||||
int dataSize = (int)lineData.size();
|
||||
sg_getLineJumpFeature_cornerMethod(
|
||||
&hLines[line][0],
|
||||
dataSize,
|
||||
line,
|
||||
cornerPara, //scale通常取bagH的1/4
|
||||
a_line_features);
|
||||
std::vector<SSG_basicFeature1D> a_line_features;
|
||||
int dataSize = (int)lineData.size();
|
||||
sg_getLineJumpFeature_cornerMethod(
|
||||
&hLines[line][0],
|
||||
dataSize,
|
||||
line,
|
||||
cornerPara, //scale通常取bagH的1/4
|
||||
a_line_features);
|
||||
|
||||
//进行配对匹配
|
||||
jumpFeatures_h.push_back(a_line_features);
|
||||
//滤除地面
|
||||
std::vector<SSG_basicFeature1D> vld_features;
|
||||
for (int i = 0, i_max = a_line_features.size(); i < i_max; i++)
|
||||
{
|
||||
//将与地面形成的跳变去除
|
||||
if (a_line_features[i].featureValue < (groundCalibPara.planeHeight - 0.25))
|
||||
{
|
||||
vld_features.push_back(a_line_features[i]);
|
||||
}
|
||||
}
|
||||
jumpFeatures_h.push_back(vld_features);
|
||||
}
|
||||
}
|
||||
|
||||
//特征生长
|
||||
//垂直方向特征生长（激光线方向）
|
||||
std::vector<SSG_featureTree> v_trees;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
if ((keSX_ScanMode_V == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode))
|
||||
{
|
||||
bool isLastLine = false;
|
||||
if (line == lineNum - 1)
|
||||
isLastLine = true;
|
||||
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_v[line];
|
||||
sg_lineFeaturesGrowing(
|
||||
line,
|
||||
isLastLine,
|
||||
a_lineJumpFeature,
|
||||
v_trees,
|
||||
growParam);
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
bool isLastLine = false;
|
||||
if (line == lineNum - 1)
|
||||
isLastLine = true;
|
||||
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_v[line];
|
||||
if (a_lineJumpFeature.size() > 0)
|
||||
int kkk = 1;
|
||||
if (line == 400)
|
||||
int kkk = 1;
|
||||
sg_lineFeaturesGrowing(
|
||||
line,
|
||||
isLastLine,
|
||||
a_lineJumpFeature,
|
||||
v_trees,
|
||||
growParam);
|
||||
}
|
||||
}
|
||||
//水平方向特征生长（扫描运动方向）
|
||||
std::vector<SSG_featureTree> h_trees;
|
||||
for (int line = 0; line < lineNum_h; line++)
|
||||
if ((keSX_ScanMode_H == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode))
|
||||
{
|
||||
if (line == 650)
|
||||
int kkk = 1;
|
||||
bool isLastLine = false;
|
||||
if (line == lineNum_h - 1)
|
||||
isLastLine = true;
|
||||
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_h[line];
|
||||
sg_lineFeaturesGrowing(
|
||||
line,
|
||||
isLastLine,
|
||||
a_lineJumpFeature,
|
||||
h_trees,
|
||||
growParam);
|
||||
for (int line = 0; line < lineNum_h; line++)
|
||||
{
|
||||
if (line == 650)
|
||||
int kkk = 1;
|
||||
bool isLastLine = false;
|
||||
if (line == lineNum_h - 1)
|
||||
isLastLine = true;
|
||||
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_h[line];
|
||||
sg_lineFeaturesGrowing(
|
||||
line,
|
||||
isLastLine,
|
||||
a_lineJumpFeature,
|
||||
h_trees,
|
||||
growParam);
|
||||
}
|
||||
}
|
||||
|
||||
//tree信息
|
||||
@ -159,6 +260,8 @@ void sx_getLapWeldPostion(
|
||||
a_treeInfo.eLineIdx = a_vTree->eLineIdx;
|
||||
a_treeInfo.roi = a_vTree->roi;
|
||||
allTreesInfo.push_back(a_treeInfo);
|
||||
|
||||
std::vector<SVzNL3DPoint> a_weld_contour;
|
||||
//在原始点云上标记，同时有Mask上标记
|
||||
for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++)
|
||||
{
|
||||
@ -172,8 +275,13 @@ void sx_getLapWeldPostion(
|
||||
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx &= 0xffff;
|
||||
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx += hvTreeIdx << 16;
|
||||
}
|
||||
a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D);
|
||||
}
|
||||
}
|
||||
std::vector<SVzNL3DPoint> a_weld;
|
||||
getWeldPoint(a_weld_contour, lapWeldParam.weldRefPoints, a_weld);
|
||||
if (a_weld.size() > 0)
|
||||
objOps.push_back(a_weld);
|
||||
hvTreeIdx++;
|
||||
}
|
||||
int hTreeStart = hvTreeIdx;
|
||||
@ -193,6 +301,8 @@ void sx_getLapWeldPostion(
|
||||
a_treeInfo.roi.top = a_hTree->roi.left;
|
||||
a_treeInfo.roi.bottom = a_hTree->roi.right;
|
||||
allTreesInfo.push_back(a_treeInfo);
|
||||
|
||||
std::vector<SVzNL3DPoint> a_weld_contour;
|
||||
//在原始点云上标记，同时有Mask上标记
|
||||
for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++)
|
||||
{
|
||||
@ -206,11 +316,41 @@ void sx_getLapWeldPostion(
|
||||
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx &= 0xffff;
|
||||
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += hvTreeIdx << 16;
|
||||
}
|
||||
a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D);
|
||||
}
|
||||
}
|
||||
std::vector<SVzNL3DPoint> a_weld;
|
||||
getWeldPoint(a_weld_contour, lapWeldParam.weldRefPoints, a_weld);
|
||||
if (a_weld.size() > 0)
|
||||
objOps.push_back(a_weld);
|
||||
|
||||
hvTreeIdx++;
|
||||
}
|
||||
int hvTreeSize = hvTreeIdx;
|
||||
|
||||
//将数据重新投射回原来的坐标系，以保持手眼标定结果正确
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
sx_lineDataR(scanLines[i], groundCalibPara.invRMatrix, -1);
|
||||
//将检测结果重新投射回原来的坐标系
|
||||
for (int i = 0, i_max = (int)objOps.size(); i < i_max; i++)
|
||||
{
|
||||
std::vector<SVzNL3DPoint>& a_weld = objOps[i];
|
||||
for (int j = 0, j_max = a_weld.size(); j < j_max; j++)
|
||||
{
|
||||
double x = a_weld[j].x * groundCalibPara.invRMatrix[0] +
|
||||
a_weld[j].y * groundCalibPara.invRMatrix[1] +
|
||||
a_weld[j].z * groundCalibPara.invRMatrix[2];
|
||||
double y = a_weld[j].x * groundCalibPara.invRMatrix[3] +
|
||||
a_weld[j].y * groundCalibPara.invRMatrix[4] +
|
||||
a_weld[j].z * groundCalibPara.invRMatrix[5];
|
||||
double z = a_weld[j].x * groundCalibPara.invRMatrix[6] +
|
||||
a_weld[j].y * groundCalibPara.invRMatrix[7] +
|
||||
a_weld[j].z * groundCalibPara.invRMatrix[8];
|
||||
a_weld[j].x = x;
|
||||
a_weld[j].y = y;
|
||||
a_weld[j].z = z;
|
||||
}
|
||||
}
|
||||
|
||||
//输出结果
|
||||
}
|
||||
@ -10,11 +10,18 @@
|
||||
#include <vector>
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
typedef enum
|
||||
{
|
||||
keSX_ScanMode_V = 0, //激光线垂直焊缝扫描
|
||||
keSX_ScanMode_H, //激光线平行焊缝扫描
|
||||
keSX_ScanMode_Both, //既有垂直扫描，也有水平扫描
|
||||
} ESX_WeldScanMode;
|
||||
typedef struct
|
||||
{
|
||||
double lapHeight;//搭接厚度
|
||||
double weldMinLen; //最小焊缝长度，用于过滤可能的虚假焊缝
|
||||
int weldRefPoints; //输出的直线焊缝的参考点，默认是2个（起点和终点）
|
||||
ESX_WeldScanMode scanMode;
|
||||
}SSX_lapWeldParam;
|
||||
|
||||
//计算一个平面调平参数。
|
||||
@ -35,6 +42,7 @@ SG_APISHARED_EXPORT void sx_getLapWeldPostion(
|
||||
const SSG_cornerParam cornerPara,
|
||||
SSG_treeGrowParam growParam,
|
||||
SSX_lapWeldParam lapWeldParam,
|
||||
SSG_planeCalibPara groundCalibPara,
|
||||
std::vector<std::vector<SVzNL3DPoint>>& objOps,
|
||||
int* errCode);
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user