博清定位算法修改备份
This commit is contained in:
jerryzeng
parent
73cee324c7
commit
1b0df92eaa
@ -649,7 +649,7 @@ void _outputRGBDScanLapWeld_RGBD(
|
||||
#define CONVERT_TO_GRID 0
|
||||
#define TEST_COMPUTE_CALIB_PARA 0
|
||||
#define TEST_COMPUTE_CORNER 1
|
||||
#define TEST_GROUP 8
|
||||
#define TEST_GROUP 9
|
||||
int main()
|
||||
{
|
||||
const char* dataPath[TEST_GROUP] = {
|
||||
@ -661,10 +661,11 @@ int main()
|
||||
"F:/ShangGu/项目/冠钦_博清科技/数据/工件点云/", //5
|
||||
"F:/ShangGu/项目/冠钦_博清科技/数据/顶板样式/", //6
|
||||
"F:/ShangGu/项目/冠钦_博清科技/定位纠偏/现场数据/0107/", //7
|
||||
"F:/ShangGu/项目/冠钦_博清科技/定位纠偏/现场数据/LaserDetectResult/", //8
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
{1,3}, {6,8}, {6,8}, {6,8}, {6,8}, {9,11}, {2,4}, {1,4}
|
||||
{1,3}, {6,8}, {6,8}, {6,8}, {6,8}, {9,11}, {2,4}, {1,4}, {1,10}
|
||||
};
|
||||
|
||||
#if CONVERT_TO_GRID
|
||||
@ -690,8 +691,9 @@ int main()
|
||||
#endif
|
||||
|
||||
#if TEST_COMPUTE_CALIB_PARA
|
||||
int cvt_grp = 8;
|
||||
char _calib_datafile[256];
|
||||
sprintf_s(_calib_datafile, "%sLaserline_1.txt", dataPath[7]);
|
||||
sprintf_s(_calib_datafile, "%s1.txt", dataPath[cvt_grp]);
|
||||
int lineNum = 0;
|
||||
float lineV = 0.0f;
|
||||
int dataCalib = 0;
|
||||
@ -727,10 +729,10 @@ int main()
|
||||
}
|
||||
//
|
||||
char calibFile[250];
|
||||
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[7]);
|
||||
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[cvt_grp]);
|
||||
_outputCalibPara(calibFile, calibPara);
|
||||
char _out_file[256];
|
||||
sprintf_s(_out_file, "%sscanData_ground_1_calib.txt", dataPath[7]);
|
||||
sprintf_s(_out_file, "%sscanData_ground_1_calib.txt", dataPath[cvt_grp]);
|
||||
int headNullLines = 0;
|
||||
_outputScanDataFile_vector(_out_file, scanData, false, &headNullLines);
|
||||
printf("%s: calib done!\n", _calib_datafile);
|
||||
@ -741,7 +743,7 @@ int main()
|
||||
const char* ver = wd_BQWorkpieceCornerVersion();
|
||||
printf("ver:%s\n", ver);
|
||||
|
||||
for (int grp = 7; grp <= 7; grp++)
|
||||
for (int grp = 8; grp <= 8; grp++)
|
||||
{
|
||||
SSG_planeCalibPara poseCalibPara;
|
||||
//初始化成单位阵
|
||||
@ -763,7 +765,7 @@ int main()
|
||||
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx =4;
|
||||
fidx =7;
|
||||
char _scan_file[256];
|
||||
if(0 == grp)
|
||||
sprintf_s(_scan_file, "%sscanData_%d_grid.txt", dataPath[grp], fidx);
|
||||
@ -771,6 +773,8 @@ int main()
|
||||
sprintf_s(_scan_file, "%s节点%d.txt", dataPath[grp], fidx);
|
||||
else if (7 == grp)
|
||||
sprintf_s(_scan_file, "%sLaserline_%d.txt", dataPath[grp], fidx);
|
||||
else if (8 == grp)
|
||||
sprintf_s(_scan_file, "%s%d.txt", dataPath[grp], fidx);
|
||||
else
|
||||
sprintf_s(_scan_file, "%s%d_LazerData_Hi229229.txt", dataPath[grp], fidx);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||
@ -800,7 +804,7 @@ int main()
|
||||
}
|
||||
#if 0
|
||||
char _out_file[256];
|
||||
sprintf_s(_out_file, "%sscanData_%d_calib.txt", dataPath[grp], fidx);
|
||||
sprintf_s(_out_file, "%s_%d_leveling.txt", dataPath[grp], fidx);
|
||||
int headNullLines = 0;
|
||||
_outputScanDataFile_vector(_out_file, scanLines, false, &headNullLines);
|
||||
#endif
|
||||
|
||||
@ -929,7 +929,7 @@ SSG_planeCalibPara _readCalibPara(char* fileName)
|
||||
#define TEST_COMPUTE_CALIB_PARA 0
|
||||
#define TEST_COMPUTE_GRASP_POS 1
|
||||
#define TEST_GROUP 1
|
||||
int main()
|
||||
void sheetPosition_test(void)
|
||||
{
|
||||
const char* dataPath[TEST_GROUP] = {
|
||||
"F:/ShangGu/项目/冠钦项目/温州优匠工品薄片抓取/温州优匠工品薄片数据/", //0
|
||||
@ -1097,5 +1097,188 @@ int main()
|
||||
|
||||
#endif
|
||||
printf("all done!\n");
|
||||
return;
|
||||
}
|
||||
|
||||
void sheetKeyHeighth_test(void)
|
||||
{
|
||||
const char* dataPath[TEST_GROUP] = {
|
||||
"F:/ShangGu/项目/吕宁项目/测高/", //0
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
{1,6} };
|
||||
|
||||
#if TEST_COMPUTE_CALIB_PARA
|
||||
int cvt_grp = 0;
|
||||
char _calib_datafile[256];
|
||||
sprintf_s(_calib_datafile, "%s样品_grid_5.txt", dataPath[cvt_grp]);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||
wdReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanLines);
|
||||
if (scanLines.size() > 0)
|
||||
{
|
||||
//getROIdata
|
||||
std::vector<std::vector< SVzNL3DPosition>> roiScanLines;
|
||||
SSG_ROIRectD roi2D = { 237.0, 340.0, -60, 6.0 };
|
||||
|
||||
getROIdata(roi2D, scanLines, roiScanLines);
|
||||
char calibFile[250];
|
||||
sprintf_s(calibFile, "%sgroundData_ROI.txt", dataPath[cvt_grp]);
|
||||
_outputScanDataFile(calibFile, roiScanLines, 0, 0, 0);
|
||||
|
||||
SSG_planeCalibPara calibPara = wd_sheetPosition_getBaseCalibPara(roiScanLines);
|
||||
//结果进行验证
|
||||
int lineNum = (int)scanLines.size();
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
if (i == 14)
|
||||
int kkk = 1;
|
||||
//行处理
|
||||
//调平，去除地面
|
||||
wd_sheetPosition_lineDataR(scanLines[i], calibPara.planeCalib, -1);// calibPara.planeHeight);
|
||||
}
|
||||
//
|
||||
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[cvt_grp]);
|
||||
_outputCalibPara(calibFile, calibPara);
|
||||
char _out_file[256];
|
||||
sprintf_s(_out_file, "%sground_grid_calib_verify.txt", dataPath[cvt_grp]);
|
||||
_outputScanDataFile(_out_file, scanLines, 0, 0, 0);
|
||||
printf("%s: calib done!\n", _calib_datafile);
|
||||
#if 1
|
||||
for (int fidx = fileIdx[cvt_grp].nMin; fidx <= fileIdx[cvt_grp].nMax; fidx++)
|
||||
{
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%s样品_grid_%d.txt", dataPath[cvt_grp], fidx);
|
||||
std::vector<std::vector< SVzNL3DPosition>> dataLines;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, dataLines);
|
||||
if (dataLines.size() == 0)
|
||||
continue;
|
||||
lineNum = (int)dataLines.size();
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
if (i == 14)
|
||||
int kkk = 1;
|
||||
//行处理
|
||||
//调平，去除地面
|
||||
wd_sheetPosition_lineDataR(dataLines[i], calibPara.planeCalib, -1);// calibPara.planeHeight);
|
||||
}
|
||||
sprintf_s(_out_file, "%s样品_grid_%d_leveling.txt", dataPath[cvt_grp], fidx);
|
||||
_outputScanDataFile(_out_file, dataLines, 0, 0, 0);
|
||||
printf("%s: leveling done!\n", _out_file);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
#if TEST_COMPUTE_GRASP_POS
|
||||
int endGroup = TEST_GROUP - 1;
|
||||
for (int grp = 0; grp <= endGroup; grp++)
|
||||
{
|
||||
SSG_planeCalibPara poseCalibPara;
|
||||
//初始化成单位阵
|
||||
poseCalibPara.planeCalib[0] = 1.0;
|
||||
poseCalibPara.planeCalib[1] = 0.0;
|
||||
poseCalibPara.planeCalib[2] = 0.0;
|
||||
poseCalibPara.planeCalib[3] = 0.0;
|
||||
poseCalibPara.planeCalib[4] = 1.0;
|
||||
poseCalibPara.planeCalib[5] = 0.0;
|
||||
poseCalibPara.planeCalib[6] = 0.0;
|
||||
poseCalibPara.planeCalib[7] = 0.0;
|
||||
poseCalibPara.planeCalib[8] = 1.0;
|
||||
poseCalibPara.planeHeight = 645.0;
|
||||
for (int i = 0; i < 9; i++)
|
||||
poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i];
|
||||
char calibFile[250];
|
||||
#if 1
|
||||
if (grp == 0)
|
||||
{
|
||||
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]);
|
||||
poseCalibPara = _readCalibPara(calibFile);
|
||||
}
|
||||
#endif
|
||||
|
||||
SWD_sheetKeyParam sheetKeyParam;
|
||||
sheetKeyParam.keySize = {8.0, 6.0}; //薄片按键大小
|
||||
sheetKeyParam.sheetSize = {41.0,11.0}; //薄片大小
|
||||
sheetKeyParam.keyToEdgeDistance = 8.0; //薄片按键距离最近的边的距离
|
||||
sheetKeyParam.sheetThickness = 0.4; //薄片厚度0.4mm
|
||||
sheetKeyParam.nominalKeyHeight = 0.5;
|
||||
|
||||
SWD_sheetAlgoParam algoParam;
|
||||
memset(&algoParam, 0, sizeof(SWD_sheetAlgoParam));
|
||||
algoParam.cornerParam.cornerTh = 40; //45度角
|
||||
algoParam.cornerParam.scale = sheetKeyParam.sheetThickness*1.25; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
||||
algoParam.cornerParam.minEndingGap = 5;
|
||||
algoParam.cornerParam.minEndingGap_z = sheetKeyParam.sheetThickness * 0.5;
|
||||
algoParam.cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平
|
||||
algoParam.cornerParam.jumpCornerTh_2 = 30;
|
||||
SSG_outlierFilterParam filterParam;
|
||||
algoParam.filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
|
||||
algoParam.filterParam.outlierTh = 5;
|
||||
SSG_treeGrowParam growParam;
|
||||
algoParam.growParam.maxLineSkipNum = 5;
|
||||
algoParam.growParam.yDeviation_max = 3.0;
|
||||
algoParam.growParam.maxSkipDistance = 3.0;
|
||||
algoParam.growParam.zDeviation_max = 3.0;// algoParam.bagParam.bagH / 2; //袋子高度1/2
|
||||
algoParam.growParam.minLTypeTreeLen = 100; //mm
|
||||
algoParam.growParam.minVTypeTreeLen = 100; //mm
|
||||
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
SWD_gripState stateMachine;
|
||||
memset(&stateMachine, 0, sizeof(SWD_gripState));
|
||||
//fidx = 4;
|
||||
int lineNum = 0;
|
||||
float lineV = 0.0f;
|
||||
int dataCalib = 0;
|
||||
int maxTimeStamp = 0;
|
||||
int clockPerSecond = 0;
|
||||
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%s样品_grid_%d.txt", dataPath[grp], fidx);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
|
||||
if (scanLines.size() == 0)
|
||||
continue;
|
||||
|
||||
long t1 = GetTickCount64();
|
||||
int errCode = 0;
|
||||
std::vector<SWD_sheetGrasper> resultObjPositions;
|
||||
wd_computeSheetKeyHeigth(
|
||||
scanLines,
|
||||
sheetKeyParam,
|
||||
poseCalibPara,
|
||||
algoParam,
|
||||
&stateMachine, //操作状态机。指示当前状态
|
||||
&errCode,
|
||||
resultObjPositions);
|
||||
long t2 = GetTickCount64();
|
||||
|
||||
char _dbg_file[256];
|
||||
#if 1
|
||||
sprintf_s(_dbg_file, "%sresult\\样品_grid_%d_result.txt", dataPath[grp], fidx);
|
||||
_outputScanDataFile_RGBD_obj(_dbg_file, scanLines, 0, 0, 0, resultObjPositions);
|
||||
sprintf_s(_dbg_file, "%sresult\\样品_grid_%d_result_img.png", dataPath[grp], fidx);
|
||||
cv::String imgName(_dbg_file);
|
||||
double rpy[3] = { -30, 15, 0 }; //{ 0,-45, 0 }; //
|
||||
//_genXOYProjectionImage(imgName, laser3DPoints, lineNum, rpy);
|
||||
#endif
|
||||
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
printf("all done!\n");
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
#if 0
|
||||
sheetPosition_test();
|
||||
#else
|
||||
sheetKeyHeighth_test();
|
||||
#endif
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
@ -24,6 +24,7 @@
|
||||
<ProjectGuid>{80cbf888-79f3-424f-a094-17edfd452e1e}</ProjectGuid>
|
||||
<RootNamespace>YouJiangsheetPositioningtest</RootNamespace>
|
||||
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
|
||||
<ProjectName>YouJiang_sheetPositioning_test</ProjectName>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||
|
||||
@ -9,7 +9,8 @@
|
||||
//version 1.2.0 : add position length output
|
||||
//version 1.2.1 : fix bugs for ver1.2.0
|
||||
//version 1.3.0 : 算法对同一brach判断添加了距离判断
|
||||
std::string m_strVersion = "1.3.0";
|
||||
//version 1.4.0 : 使用轮廓点拐角判断目标类型
|
||||
std::string m_strVersion = "1.4.0";
|
||||
const char* wd_BQWorkpieceCornerVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -585,6 +586,256 @@ bool checkSameBranch(SWD_polarPt& corner_1, SWD_polarPt& corner_2,
|
||||
|
||||
}
|
||||
|
||||
//计算封闭序列（首尾相连）在XOY平面内前向角和后向角以及拐角。 前向角：序号+； 后向角：序号-
|
||||
void _computeClosedPntListDirCorners(std::vector<SWD_polarPt>& polarPoints, double scale, std::vector< SSG_dirCornerAngle>& dirCornerAngles)
|
||||
{
|
||||
int pntSize = (int)polarPoints.size();
|
||||
//std::fill(dirCornerAngles.begin(), dirCornerAngles.end(), SSG_dirCornerAngle{ 0, 0, 0, 0, 0, 0 });
|
||||
dirCornerAngles.resize(pntSize);
|
||||
for (int i = 0; i < pntSize; i++)
|
||||
{
|
||||
memset(&dirCornerAngles[i], 0, sizeof(SSG_dirCornerAngle));
|
||||
SWD_polarPt& a_polarPt = polarPoints[i];
|
||||
//前向寻找
|
||||
int minus_i = -1;
|
||||
for (int loop = i - 1; loop >= i-pntSize; loop--)
|
||||
{
|
||||
int j = loop >= 0 ? loop : (loop + pntSize);
|
||||
double dist = sqrt(pow(polarPoints[i].x - polarPoints[j].x, 2) +
|
||||
pow(polarPoints[i].y - polarPoints[j].y, 2));
|
||||
if (dist >= scale)
|
||||
{
|
||||
minus_i = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//后向寻找
|
||||
int plus_i = -1;
|
||||
for (int loop = i + 1; loop < i+pntSize; loop++)
|
||||
{
|
||||
int j = loop % pntSize;
|
||||
double dist = sqrt(pow(polarPoints[i].x - polarPoints[j].x, 2) +
|
||||
pow(polarPoints[i].y - polarPoints[j].y, 2));
|
||||
if (dist >= scale)
|
||||
{
|
||||
plus_i = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//计算拐角
|
||||
if ((minus_i >= 0) && (plus_i >= 0))
|
||||
{
|
||||
double backwardAngle = atan2(polarPoints[i].y - polarPoints[minus_i].y, polarPoints[i].x - polarPoints[minus_i].x) * 180.0 / PI;
|
||||
double forwardAngle = atan2(polarPoints[plus_i].y - polarPoints[i].y, polarPoints[plus_i].x - polarPoints[i].x) * 180.0 / PI;
|
||||
dirCornerAngles[i].forwardAngle = forwardAngle;
|
||||
dirCornerAngles[i].backwardAngle = backwardAngle;
|
||||
|
||||
double corner = forwardAngle - backwardAngle;
|
||||
if (corner < -180)
|
||||
corner += 360;
|
||||
else if (corner > 180)
|
||||
corner = corner - 360;
|
||||
dirCornerAngles[i].corner = corner; //图像坐标系与正常坐标系y方向相反，所以有“-”号
|
||||
dirCornerAngles[i].pntIdx = i;
|
||||
dirCornerAngles[i].forward_pntIdx = plus_i;
|
||||
dirCornerAngles[i].backward_pntIdx = minus_i;
|
||||
dirCornerAngles[i].point = polarPoints[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//提取corner极值（较早实现函数可以使用此函数进行代码优化）
|
||||
void _searchPlusCornerPeaks(
|
||||
std::vector< SSG_dirCornerAngle>& corners,
|
||||
std::vector< SSG_dirCornerAngle>& cornerPlusPeaks
|
||||
)
|
||||
{
|
||||
std::vector<SSG_dirCornerAngle> peakCorners;
|
||||
int cornerSize = (int)corners.size();
|
||||
//搜索拐角极值
|
||||
int _state = 0;
|
||||
int pre_i = -1;
|
||||
int sEdgePtIdx = -1;
|
||||
int eEdgePtIdx = -1;
|
||||
SSG_dirCornerAngle* pre_data = NULL;
|
||||
for (int i = 0, i_max = cornerSize; i < i_max; i++)
|
||||
{
|
||||
if (i == 275)
|
||||
int kkk = 1;
|
||||
SSG_dirCornerAngle* curr_data = &corners[i];
|
||||
if (NULL == pre_data)
|
||||
{
|
||||
sEdgePtIdx = i;
|
||||
eEdgePtIdx = i;
|
||||
pre_data = curr_data;
|
||||
pre_i = i;
|
||||
continue;
|
||||
}
|
||||
|
||||
eEdgePtIdx = i;
|
||||
double cornerDiff = curr_data->corner - pre_data->corner;
|
||||
switch (_state)
|
||||
{
|
||||
case 0: //初态
|
||||
if (cornerDiff < 0) //下降
|
||||
{
|
||||
_state = 2;
|
||||
}
|
||||
else if (cornerDiff > 0) //上升
|
||||
{
|
||||
_state = 1;
|
||||
}
|
||||
break;
|
||||
case 1: //上升
|
||||
if (cornerDiff < 0) //下降
|
||||
{
|
||||
if (pre_data->corner > 10) //使用10度作为门限，滤除点波动导致的角度变化
|
||||
peakCorners.push_back(*pre_data);
|
||||
_state = 2;
|
||||
}
|
||||
break;
|
||||
case 2: //下降
|
||||
if (cornerDiff > 0) // 上升
|
||||
_state = 1;
|
||||
break;
|
||||
default:
|
||||
_state = 0;
|
||||
break;
|
||||
}
|
||||
pre_data = curr_data;
|
||||
pre_i = i;
|
||||
}
|
||||
//注意：最后一个不处理，为基座位置
|
||||
|
||||
//极小值点（峰顶）
|
||||
//极值比较，在尺度窗口下寻找局部极值点
|
||||
for (int i = 0, i_max = (int)peakCorners.size(); i < i_max; i++)
|
||||
{
|
||||
bool isPeak = true;
|
||||
SSG_dirCornerAngle& a_dirAngle = peakCorners[i];
|
||||
int curr_dist1 = a_dirAngle.pntIdx - a_dirAngle.backward_pntIdx;
|
||||
if (curr_dist1 < 0)
|
||||
curr_dist1 += cornerSize;
|
||||
int curr_dist2 = a_dirAngle.forward_pntIdx - a_dirAngle.pntIdx;
|
||||
if (curr_dist2 < 0)
|
||||
curr_dist2 += cornerSize;
|
||||
|
||||
//minus方向寻找
|
||||
int minus_i = i;
|
||||
while (1)
|
||||
{
|
||||
minus_i--;
|
||||
if (minus_i < 0)
|
||||
minus_i += i_max;
|
||||
SSG_dirCornerAngle& minus_dirAngle = peakCorners[minus_i];
|
||||
int pntDist_0 = a_dirAngle.pntIdx - minus_dirAngle.pntIdx;
|
||||
if (pntDist_0 < 0)
|
||||
pntDist_0 += cornerSize;
|
||||
if (pntDist_0 > curr_dist1)
|
||||
break;
|
||||
|
||||
if (a_dirAngle.corner < minus_dirAngle.corner)
|
||||
{
|
||||
isPeak = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//plus方向寻找
|
||||
int plus_i = i;
|
||||
while (1)
|
||||
{
|
||||
plus_i++;
|
||||
if (plus_i >= i_max)
|
||||
plus_i = 0;
|
||||
SSG_dirCornerAngle& plus_dirAngle = peakCorners[plus_i];
|
||||
int pntDist_0 = plus_dirAngle.pntIdx - a_dirAngle.pntIdx;
|
||||
if (pntDist_0 < 0)
|
||||
pntDist_0 += cornerSize;
|
||||
if (pntDist_0 > curr_dist2)
|
||||
break;
|
||||
|
||||
if (a_dirAngle.corner < plus_dirAngle.corner)
|
||||
{
|
||||
isPeak = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (true == isPeak)
|
||||
{
|
||||
double corner_curr = a_dirAngle.corner;
|
||||
double corner_1 = corners[a_dirAngle.forward_pntIdx].corner;
|
||||
double corner_2 = corners[a_dirAngle.backward_pntIdx].corner;
|
||||
double diff_1 = corner_curr - corner_1;
|
||||
double diff_2 = corner_curr - corner_2;
|
||||
if ((diff_1 > corner_curr / 2) && (diff_2 > corner_curr / 2))
|
||||
cornerPlusPeaks.push_back(a_dirAngle);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//根据轮廓的角点计算Branch， 并判断是否缺角。缺角时通过拟合方式补全
|
||||
// std::vector<SWD_polarPt>& polarPoints: 点的循环对列
|
||||
//branchCornerAngle: 支持寻找不同角度的Branch，由branchCornerAngle指定。90度为直角。
|
||||
//angleTh: 角度误差范围。比如名义为90的直角，实际加工会有误差。
|
||||
//cutLineDeviationTh:以截角两点连线，截角上点到直线的最大偏离。保证截角是直线
|
||||
void _computeBranchesFromCornerPeaks(std::vector<SWD_polarPt>& polarPoints, std::vector< SSG_dirCornerAngle>& cornerPlusPeaks, std::vector<SWD_branchInfo>& branches, double branchCornerAngle, double angleTh, double cutLineDeviationTh)
|
||||
{
|
||||
int ringBuffSize = (int)polarPoints.size();
|
||||
//判断是否有缺角
|
||||
int size_peaks = (int)cornerPlusPeaks.size();
|
||||
std::vector<int> cutPairFlags;
|
||||
cutPairFlags.resize(size_peaks);
|
||||
std::fill(cutPairFlags.begin(), cutPairFlags.end(), 0);
|
||||
int cutPairID = 1;
|
||||
for (int i = 0; i < size_peaks; i++)
|
||||
{
|
||||
if (cutPairFlags[i] > 0)
|
||||
continue;
|
||||
|
||||
SSG_dirCornerAngle& corner_1 = cornerPlusPeaks[i];
|
||||
int tmpIdx = (i + 1) % size_peaks;
|
||||
SSG_dirCornerAngle& corner_2 = cornerPlusPeaks[tmpIdx];
|
||||
if (corner_2.pntIdx < 0)
|
||||
continue;
|
||||
|
||||
double sumCorner = corner_1.corner + corner_2.corner;
|
||||
double angleDiff = abs(sumCorner - branchCornerAngle);
|
||||
if (angleDiff < angleTh)
|
||||
{
|
||||
//偏离度判断
|
||||
//计算两点连线
|
||||
double a, b, c;
|
||||
compute2ptLine_2(
|
||||
corner_1.point.x, corner_1.point.y,
|
||||
corner_2.point.x, corner_2.point.y,
|
||||
&a, &b, &c);
|
||||
double tmp = sqrt(a * a + b * b);
|
||||
a = a / tmp;
|
||||
b = b / tmp;
|
||||
c = c / tmp;
|
||||
|
||||
double maxDist = 0;
|
||||
int number = corner_2.pntIdx - corner_1.pntIdx;
|
||||
if (number < 0)
|
||||
number += ringBuffSize;
|
||||
for (int m = 0; m < number; m++)
|
||||
{
|
||||
int idx = (m + corner_1.pntIdx) % ringBuffSize;
|
||||
double dist = abs(a * polarPoints[idx].x + b * polarPoints[m].y + c);
|
||||
if (maxDist < dist)
|
||||
maxDist = dist;
|
||||
}
|
||||
if (maxDist < cutLineDeviationTh) //判断为截角
|
||||
{
|
||||
cutPairFlags[i] = cutPairID;
|
||||
cutPairFlags[i+1] = cutPairID;
|
||||
cutPairID++;
|
||||
}
|
||||
}
|
||||
}
|
||||
//迭代一次，对于循环Buff, 需要重新检查首尾结合部
|
||||
|
||||
}
|
||||
|
||||
SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
@ -876,6 +1127,17 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
for (int pi = 0; pi < contourPtSize; pi++)
|
||||
polarPoints[pi].cptIndex = pi; // index
|
||||
|
||||
//计算有序边缘点的角度变化 2026.03.06
|
||||
std::vector< SSG_dirCornerAngle> dirCornerAngles;
|
||||
double scale = 20.0;
|
||||
_computeClosedPntListDirCorners(polarPoints, scale, dirCornerAngles);
|
||||
//提取方向角拐点(正）极值
|
||||
std::vector< SSG_dirCornerAngle> cornerPlusPeaks;
|
||||
_searchPlusCornerPeaks(dirCornerAngles, cornerPlusPeaks);
|
||||
//使用R过滤，判断是否缺角
|
||||
std::vector<SWD_branchInfo> branches;
|
||||
_computeBranchesFromCornerPeaks(cornerPlusPeaks, branches);
|
||||
|
||||
//提取R极值点
|
||||
double minR = -1, maxR = -1; //计算最小和最大的R，用以区分有没有分支。minR和maxR相差小时，为圆形或8角形，没有分支
|
||||
int minRPos = -1;
|
||||
@ -1035,6 +1297,9 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
branchPeakInfo.erase(branchPeakInfo.begin() + m);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
int branchNum = (int)branchPeaks.size();
|
||||
if (branchNum == 2)
|
||||
{
|
||||
@ -1169,6 +1434,9 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
debug_contours.push_back(a_branchDebug);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
workpieceCorners.workpieceType = workpieceType;
|
||||
if (workpieceType == 1) //4个branch
|
||||
{
|
||||
|
||||
@ -512,7 +512,8 @@ SG_APISHARED_EXPORT bool leastSquareParabolaFitEigen(
|
||||
|
||||
//计算Z均值
|
||||
SG_APISHARED_EXPORT double computeMeanZ(std::vector< SVzNL3DPoint>& pts);
|
||||
|
||||
SG_APISHARED_EXPORT double computeROIMeanZ(std::vector<std::vector< SVzNL3DPosition>>& scanLines, SVzNLRect roi);
|
||||
SG_APISHARED_EXPORT void computeROIZCutLabel(std::vector<std::vector< SVzNL3DPosition>>& scanLines, SVzNLRect roi, double cutZ, int labelID);
|
||||
//计算角度差值，在0-180度范围
|
||||
SG_APISHARED_EXPORT double computeAngleDiff(double theta1, double theta2);
|
||||
|
||||
|
||||
@ -100,6 +100,12 @@ typedef struct
|
||||
double bottom;
|
||||
}SSG_ROIRectD;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double width;
|
||||
double height;
|
||||
}SSG_size2D;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
SVzNL3DPoint center;
|
||||
@ -526,6 +532,20 @@ typedef struct
|
||||
double invRMatrix[9]; //旋转矩阵，回到原坐标系
|
||||
}SSG_planeCalibPara;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int pkId;
|
||||
int lineIdx;
|
||||
int ptIdx;
|
||||
int cptIndex; //圆周扫描上的点序
|
||||
//double cornerAngle; //以点为中心的两侧弦的夹角
|
||||
double R;
|
||||
double angle;
|
||||
double x;
|
||||
double y;
|
||||
double z;
|
||||
}SWD_polarPt;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int pntIdx;
|
||||
@ -541,6 +561,17 @@ typedef struct
|
||||
double backward_z;
|
||||
}SSG_pntDirAngle;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double forwardAngle; //前向角
|
||||
double backwardAngle; //后向角
|
||||
double corner; //拐角
|
||||
int pntIdx;
|
||||
int forward_pntIdx;
|
||||
int backward_pntIdx;
|
||||
SWD_polarPt point;
|
||||
}SSG_dirCornerAngle;
|
||||
|
||||
// 图像数据结构
|
||||
typedef struct {
|
||||
int width;
|
||||
@ -562,20 +593,6 @@ typedef struct
|
||||
SVzNL3DPoint pt2;
|
||||
}SWD_3DPointPair;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int pkId;
|
||||
int lineIdx;
|
||||
int ptIdx;
|
||||
int cptIndex; //圆周扫描上的点序
|
||||
//double cornerAngle; //以点为中心的两侧弦的夹角
|
||||
double R;
|
||||
double angle;
|
||||
double x;
|
||||
double y;
|
||||
double z;
|
||||
}SWD_polarPt;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int cptIndex;
|
||||
|
||||
@ -298,6 +298,40 @@ double computeMeanZ(std::vector< SVzNL3DPoint>& pts)
|
||||
return 0;
|
||||
}
|
||||
|
||||
double computeROIMeanZ(std::vector<std::vector< SVzNL3DPosition>>& scanLines, SVzNLRect roi)
|
||||
{
|
||||
int vldNum = 0;
|
||||
double sumZ = 0;
|
||||
for (int line = roi.left; line <= roi.right; line++)
|
||||
{
|
||||
for (int i = roi.top; i <= roi.bottom; i++)
|
||||
{
|
||||
if (scanLines[line][i].pt3D.z > 1e-4)
|
||||
{
|
||||
sumZ += scanLines[line][i].pt3D.z;
|
||||
vldNum++;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (vldNum > 0)
|
||||
return (sumZ / vldNum);
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
|
||||
void computeROIZCutLabel(std::vector<std::vector< SVzNL3DPosition>>& scanLines, SVzNLRect roi, double cutZ, int labelID)
|
||||
{
|
||||
for (int line = roi.left; line <= roi.right; line++)
|
||||
{
|
||||
for (int i = roi.top; i <= roi.bottom; i++)
|
||||
{
|
||||
if ( (scanLines[line][i].pt3D.z > 1e-4) && (scanLines[line][i].pt3D.z < cutZ))
|
||||
scanLines[line][i].nPointIdx = labelID;
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
SVzNL3DPoint computeLineCrossPt_abs(double a1, double b1, double c1, double a2, double b2, double c2)
|
||||
{
|
||||
SVzNL3DPoint crossPt;
|
||||
|
||||
@ -30,16 +30,14 @@ void wd_sheetPosition_lineDataR(
|
||||
lineDataRT_vector(a_line, camPoseR, groundH);
|
||||
}
|
||||
|
||||
//薄片定位
|
||||
//使用语义模块匹配：（1）提取特征（2）基于特征匹配薄片
|
||||
void wd_YouJiang_getSheetPosition(
|
||||
//提取边缘封闭目标的边缘特征，输出聚类簇
|
||||
void wd_computeClosedEdgeClusters(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SWD_sheetTemplateParam templateParam,
|
||||
const SSG_planeCalibPara groundCalibPara,
|
||||
const SWD_sheetAlgoParam algoParam,
|
||||
SWD_gripState* opState, //操作状态机。指示当前状态
|
||||
int* errCode,
|
||||
std::vector<SWD_sheetGrasper>& resultObjPositions)
|
||||
std::vector<std::vector< SVzNL2DPoint>>& clusters, //只记录位置
|
||||
std::vector<SVzNL3DRangeD>& clustersRoi3D)
|
||||
{
|
||||
int lineNum = (int)scanLines.size();
|
||||
if (lineNum == 0)
|
||||
@ -51,7 +49,7 @@ void wd_YouJiang_getSheetPosition(
|
||||
bool isGridData = true;
|
||||
int linePtNum = (int)scanLines[0].size();
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
{
|
||||
if (linePtNum != (int)scanLines[i].size())
|
||||
isGridData = false;//行处理
|
||||
//调平，去除地面
|
||||
@ -198,8 +196,6 @@ void wd_YouJiang_getSheetPosition(
|
||||
|
||||
//聚类
|
||||
//采用迭代思想，回归思路进行高效聚类
|
||||
std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
|
||||
std::vector<SVzNL3DRangeD> clustersRoi3D;
|
||||
int clusterID = 1;
|
||||
int clusterCheckWin = 3;
|
||||
for (int y = 0; y < lineNum_h_raw; y++)
|
||||
@ -236,8 +232,32 @@ void wd_YouJiang_getSheetPosition(
|
||||
clusterID++;
|
||||
}
|
||||
}
|
||||
int clusterNum = (int)clusters.size();
|
||||
}
|
||||
|
||||
|
||||
//薄片定位
|
||||
//使用语义模块匹配：（1）提取特征（2）基于特征匹配薄片
|
||||
void wd_YouJiang_getSheetPosition(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SWD_sheetTemplateParam templateParam,
|
||||
const SSG_planeCalibPara groundCalibPara,
|
||||
const SWD_sheetAlgoParam algoParam,
|
||||
SWD_gripState* opState, //操作状态机。指示当前状态
|
||||
int* errCode,
|
||||
std::vector<SWD_sheetGrasper>& resultObjPositions)
|
||||
{
|
||||
std::vector<std::vector< SVzNL2DPoint>> clusters;//只记录位置
|
||||
std::vector<SVzNL3DRangeD> clustersRoi3D;
|
||||
//提取边缘封闭目标的边缘特征，输出聚类簇
|
||||
wd_computeClosedEdgeClusters(
|
||||
scanLines,
|
||||
groundCalibPara,
|
||||
algoParam,
|
||||
errCode,
|
||||
clusters, //只记录位置
|
||||
clustersRoi3D);
|
||||
|
||||
int clusterNum = (int)clusters.size();
|
||||
//投影和标注。投影的目的是用于模板匹配时的特征处理。以1mm为单位.记录clusterID
|
||||
SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(scanLines);
|
||||
double projectionScale = 1.0; //以1mm为投影尺度
|
||||
@ -250,12 +270,14 @@ void wd_YouJiang_getSheetPosition(
|
||||
for (int i = 0; i < clusterNum; i++)
|
||||
{
|
||||
std::vector< SVzNL2DPoint>& a_cluster = clusters[i];
|
||||
for (int j = 0; j < (int)clusters.size(); j++)
|
||||
for (int j = 0; j < (int)a_cluster.size(); j++)
|
||||
{
|
||||
SVzNL2DPoint& a_pt = a_cluster[i];
|
||||
SVzNL2DPoint& a_pt = a_cluster[j];
|
||||
SVzNL3DPosition& a_pt3d = scanLines[a_pt.x][a_pt.y];
|
||||
int px = (int)((a_pt3d.pt3D.x - roi3D.xRange.min) / projectionScale);
|
||||
int py = (int)((a_pt3d.pt3D.y - roi3D.yRange.min) / projectionScale);
|
||||
if ((px < 0) || (px >= projecction_xw) || (py < 0) || (py >= projecction_yh))
|
||||
int kkk = 1;
|
||||
projectionClusters[px][py] = i + 1; //clusterID = index + 1
|
||||
}
|
||||
}
|
||||
@ -288,4 +310,113 @@ void wd_YouJiang_getSheetPosition(
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
cv::RotatedRect rect;
|
||||
cv::Point2f vertices[4];
|
||||
}_RectParam;
|
||||
//薄片按键高度测量
|
||||
void wd_computeSheetKeyHeigth(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SWD_sheetKeyParam sheetKeyParam,
|
||||
const SSG_planeCalibPara groundCalibPara,
|
||||
const SWD_sheetAlgoParam algoParam,
|
||||
SWD_gripState* opState, //操作状态机。指示当前状态
|
||||
int* errCode,
|
||||
std::vector<SWD_sheetGrasper>& resultObjPositions)
|
||||
{
|
||||
std::vector<std::vector< SVzNL2DPoint>> clusters;//只记录位置
|
||||
std::vector<SVzNL3DRangeD> clustersRoi3D;
|
||||
//提取边缘封闭目标的边缘特征，输出聚类簇
|
||||
wd_computeClosedEdgeClusters(
|
||||
scanLines,
|
||||
groundCalibPara,
|
||||
algoParam,
|
||||
errCode,
|
||||
clusters, //只记录位置
|
||||
clustersRoi3D);
|
||||
int clusterNum = (int)clusters.size();
|
||||
|
||||
//聚类结果分析
|
||||
//根据大小确定薄片
|
||||
const double sameSizeTh_w = sheetKeyParam.sheetSize.width * 0.05;
|
||||
const double sameSizeTh_h = sheetKeyParam.sheetSize.height * 0.05;
|
||||
const double zAreaWin = 5;
|
||||
|
||||
std::vector<_RectParam> clustersRect;
|
||||
clustersRect.resize(clusters.size());
|
||||
#if 1
|
||||
std::vector<int> validIndice;
|
||||
|
||||
for (int i = 0; i < clusterNum; i++)
|
||||
{
|
||||
std::vector< SVzNL2DPoint>& a_cluster = clusters[i];
|
||||
if (a_cluster.size() < 4)
|
||||
continue;
|
||||
//计算最小外接矩
|
||||
std::vector<cv::Point2f> points;
|
||||
SVzNLRect roi = { -1,-1,-1,-1 };
|
||||
for (int j = 0; j < (int)a_cluster.size(); j++)
|
||||
{
|
||||
SVzNL2DPoint& a_pt = a_cluster[j];
|
||||
SVzNL3DPosition& a_pt3d = scanLines[a_pt.x][a_pt.y];
|
||||
//生成ROI
|
||||
if (roi.left < 0)
|
||||
roi = { a_pt.x ,a_pt.x , a_pt.y, a_pt.y };
|
||||
else
|
||||
{
|
||||
if (roi.left > a_pt.x)
|
||||
roi.left = a_pt.x;
|
||||
if (roi.right < a_pt.x)
|
||||
roi.right = a_pt.x;
|
||||
if (roi.top > a_pt.y)
|
||||
roi.top = a_pt.y;
|
||||
if (roi.bottom < a_pt.y)
|
||||
roi.bottom = a_pt.y;
|
||||
}
|
||||
float x = (float)a_pt3d.pt3D.x;
|
||||
float y = (float)a_pt3d.pt3D.y;
|
||||
points.push_back(cv::Point2f(x, y));
|
||||
}
|
||||
if (points.size() == 0)
|
||||
continue;
|
||||
|
||||
// 最小外接矩形
|
||||
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;
|
||||
}
|
||||
|
||||
int cx = (roi.left + roi.right) / 2;
|
||||
int cy = (roi.top + roi.bottom) / 2;
|
||||
//判别
|
||||
double w_diff = abs(width - sheetKeyParam.sheetSize.width);
|
||||
double h_diff = abs(height - sheetKeyParam.sheetSize.height);
|
||||
if ((w_diff < sameSizeTh_w) && (h_diff < sameSizeTh_h))
|
||||
{
|
||||
//寻找到目标
|
||||
//取中心点，以中心点附近区域计算Z
|
||||
int cx = (roi.left + roi.right) / 2;
|
||||
int cy = (roi.top + roi.bottom) / 2;
|
||||
SVzNLRect meanZRoi = { cx - zAreaWin , cx + zAreaWin , cy - zAreaWin , cy + zAreaWin };
|
||||
double meanZ = computeROIMeanZ(scanLines, meanZRoi);
|
||||
//以Z值作为门限进行切割
|
||||
double cutZ = meanZ - sheetKeyParam.nominalKeyHeight * 0.75;
|
||||
int labelID = i + 100; //ID从100开始
|
||||
computeROIZCutLabel(scanLines, roi, cutZ, labelID);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
}
|
||||
@ -17,6 +17,15 @@ typedef struct
|
||||
double centerBossHeight; //薄片中央凸起高度
|
||||
}SWD_sheetTemplateParam;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
SSG_size2D keySize; //薄片按键大小
|
||||
SSG_size2D sheetSize; //薄片大小
|
||||
double keyToEdgeDistance; //按键距最近的边的距离
|
||||
double sheetThickness; //薄片厚度
|
||||
double nominalKeyHeight; //名义按键高度
|
||||
}SWD_sheetKeyParam;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
SSG_outlierFilterParam filterParam;
|
||||
@ -64,4 +73,14 @@ SG_APISHARED_EXPORT void wd_YouJiang_getSheetPosition(
|
||||
const SWD_sheetAlgoParam algoParam,
|
||||
SWD_gripState* opState, //操作状态机。指示当前状态
|
||||
int* errCode,
|
||||
std::vector<SWD_sheetGrasper>& resultObjPositions);
|
||||
|
||||
//薄片按键高度测量
|
||||
SG_APISHARED_EXPORT void wd_computeSheetKeyHeigth(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SWD_sheetKeyParam sheetKeyParam,
|
||||
const SSG_planeCalibPara groundCalibPara,
|
||||
const SWD_sheetAlgoParam algoParam,
|
||||
SWD_gripState* opState, //操作状态机。指示当前状态
|
||||
int* errCode,
|
||||
std::vector<SWD_sheetGrasper>& resultObjPositions);
|
||||
Loading…
x
Reference in New Issue
Block a user