jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

BQ_workpieceCornerExtract version 1.3.0 :

Browse Source 
算法对同一brach判断添加了距离判断
This commit is contained in:
jerryzeng 2026-01-08 21:40:43 +08:00
parent 4f24efc6b6
commit 7204f3efca
2 changed files with 59 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp
View File

@ -649,21 +649,22 @@ void _outputRGBDScanLapWeld_RGBD(
#define CONVERT_TO_GRID 0
#define TEST_COMPUTE_CALIB_PARA 0
#define TEST_COMPUTE_CORNER 1
#define TEST_GROUP 7
#define TEST_GROUP 8
int main()
{
const char* dataPath[TEST_GROUP] = {
"F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\", //0
"F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度1\\", //1
"F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度2\\", //2
"F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度3\\", //3
"F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度4\\", //4
"F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\", //5
"F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\顶板样式\\", //6
"F:/ShangGu/项目/冠钦_博清科技/数据/", //0
"F:/ShangGu/项目/冠钦_博清科技/数据/工件点云/角度1/", //1
"F:/ShangGu/项目/冠钦_博清科技/数据/工件点云/角度2/", //2
"F:/ShangGu/项目/冠钦_博清科技/数据/工件点云/角度3/", //3
"F:/ShangGu/项目/冠钦_博清科技/数据/工件点云/角度4/", //4
"F:/ShangGu/项目/冠钦_博清科技/数据/工件点云/", //5
"F:/ShangGu/项目/冠钦_博清科技/数据/顶板样式/", //6
"F:/ShangGu/项目/冠钦_博清科技/定位纠偏/现场数据/0107/", //7
};
SVzNLRange fileIdx[TEST_GROUP] = {
{1,3}, {6,8}, {6,8}, {6,8}, {6,8}, {9,11}, {2,4}
{1,3}, {6,8}, {6,8}, {6,8}, {6,8}, {9,11}, {2,4}, {1,4}
};
#if CONVERT_TO_GRID
@ -690,7 +691,7 @@ int main()
#if TEST_COMPUTE_CALIB_PARA
char _calib_datafile[256];
sprintf_s(_calib_datafile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\顶板样式\\节点4.txt");
sprintf_s(_calib_datafile, "%sLaserline_1.txt", dataPath[7]);
int lineNum = 0;
float lineV = 0.0f;
int dataCalib = 0;
@ -726,10 +727,10 @@ int main()
}
//
char calibFile[250];
sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\顶板样式\\ground_calib_para.txt");
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[7]);
_outputCalibPara(calibFile, calibPara);
char _out_file[256];
sprintf_s(_out_file, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\顶板样式\\scanData_ground_1_calib.txt");
sprintf_s(_out_file, "%sscanData_ground_1_calib.txt", dataPath[7]);
int headNullLines = 0;
_outputScanDataFile_vector(_out_file, scanData, false, &headNullLines);
printf("%s: calib done!\n", _calib_datafile);
@ -740,7 +741,7 @@ int main()
const char* ver = wd_BQWorkpieceCornerVersion();
printf("ver:%s\n", ver);
for (int grp = 5; grp <= 6; grp++)
for (int grp = 6; grp <= 7; grp++)
{
SSG_planeCalibPara poseCalibPara;
//初始化成单位阵
@ -768,6 +769,8 @@ int main()
sprintf_s(_scan_file, "%sscanData_%d_grid.txt", dataPath[grp], fidx);
else if(6 == grp)
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
sprintf_s(_scan_file, "%s%d_LazerData_Hi229229.txt", dataPath[grp], fidx);
std::vector<std::vector< SVzNL3DPosition>> scanLines;
@ -820,6 +823,9 @@ int main()
growParam.minLTypeTreeLen = 100; //mm
growParam.minVTypeTreeLen = 100; //mm
SSX_BQworkpiecePara workpieceParam;
if(grp == 7)
workpieceParam.lineLen = 120.0; //直线段长度
else
workpieceParam.lineLen = 160.0; //直线段长度
int errCode = 0;
std::vector<SSX_debugInfo> debug_conturs;

41
sourceCode/BQ_workpieceCornerExtraction.cpp
View File

@ -8,7 +8,8 @@
//version 1.1.0 : base version release to customer, output corner coordinate
//version 1.2.0 : add position length output
//version 1.2.1 : fix bugs for ver1.2.0
std::string m_strVersion = "1.2.1";
//version 1.3.0 : 算法对同一brach判断添加了距离判断
std::string m_strVersion = "1.3.0";
const char* wd_BQWorkpieceCornerVersion(void)
{
return m_strVersion.c_str();
@ -552,6 +553,39 @@ SVzNL3DPoint computeEdgeCross(double angle, std::vector<SWD_polarPt>& polarPoint
return a_pt;
}
bool checkSameBranch(SWD_polarPt& corner_1, SWD_polarPt& corner_2,
std::vector<SWD_polarPt>& polarPoints,
double center_x, double center_y)
{
int size = (int)polarPoints.size();
int midPtIdx;
if (corner_2.cptIndex > corner_1.cptIndex)
midPtIdx = (corner_1.cptIndex + corner_2.cptIndex) / 2;
else
{
midPtIdx = (size - corner_1.cptIndex + corner_2.cptIndex) / 2 + corner_1.cptIndex;
if (midPtIdx >= size)
midPtIdx = midPtIdx - size;
}
SWD_polarPt& midPt = polarPoints[midPtIdx];
double a1, b1, c1;
compute2ptLine_2(center_x, center_y, midPt.x, midPt.y, &a1, &b1, &c1);
double a2, b2, c2;
compute2ptLine_2(corner_1.x, corner_1.y, corner_2.x, corner_2.y, &a2, &b2, &c2);
//计算交点
SVzNL3DPoint crossPt = computeLineCrossPt_abs(a1, b1, c1, a2, b2, c2);
double len = sqrt(pow(crossPt.x - center_x, 2) + pow(crossPt.y - center_y, 2));
double dist_diff = midPt.R / len;
if ( (dist_diff > 0.95) &&(dist_diff < 1.05))
return true;
else
return false;
}
SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSG_cornerParam cornerPara,
@ -967,10 +1001,13 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
//if (polarPeakInfo[m].cornerDir == 2) //ÄæʱÕë¡£
{
int nxtIdx = (m + 1)%pkSize;
bool isSameBranch = checkSameBranch(validPolarRPeakPts[m], validPolarRPeakPts[nxtIdx],
polarPoints, center_x, center_y);
double angleDiff = computeAngleDiff(validPolarRPeakPts[nxtIdx].angle, validPolarRPeakPts[m].angle);
if (angleDiff < 0)
angleDiff += 360;
if (angleDiff < 90) //为同一branch，合并，只保留一个
if ( (angleDiff < 90) && (true == isSameBranch)) //为同一branch，合并，只保留一个
{
validPolarRPeakPts[nxtIdx].pkId = validPolarRPeakPts[m].pkId; //pair
validPolarRPeakPts[nxtIdx].cptIndex = -1;