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

BQ_workpieceCornerExtraction version 1.4.1 :

Browse Source 
(1)使用聚类方法寻找准确轮廓点（2）使用拐角和R极值方法判断目标类型
This commit is contained in:
jerryzeng 2026-03-19 00:43:39 +08:00
parent 4acb751647
commit 0231f54828
9 changed files with 1394 additions and 209 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

@ -607,13 +607,19 @@ void _outputRGBDScanLapWeld_RGBD(
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
}
}
//加一个点，用于跳过显示工具bug
float x = (float)debugData[0].edge[0].x;
float y = (float)debugData[0].edge[0].y;
float z = (float)debugData[0].edge[0].z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
if (debugData.size() > 0)
{
if (debugData[0].edge != NULL)
{
//加一个点，用于跳过显示工具bug
float x = (float)debugData[0].edge[0].x;
float y = (float)debugData[0].edge[0].y;
float z = (float)debugData[0].edge[0].z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
}
}
}
//显示拟合直线
rgb = { 255, 0, 0 };
@ -743,7 +749,7 @@ int main()
const char* ver = wd_BQWorkpieceCornerVersion();
printf("ver:%s\n", ver);
for (int grp = 7; grp <= 7; grp++)
for (int grp = 6; grp <= 8; grp++)
{
SSG_planeCalibPara poseCalibPara;
//初始化成单位阵
@ -765,7 +771,7 @@ int main()
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
{
//fidx =2;
//fidx =8;
char _scan_file[256];
if(0 == grp)
sprintf_s(_scan_file, "%sscanData_%d_grid.txt", dataPath[grp], fidx);
@ -821,8 +827,8 @@ int main()
filterParam.outlierTh = 5;
SSG_treeGrowParam growParam;
growParam.maxLineSkipNum = 10;
growParam.yDeviation_max = 5.0;
growParam.maxSkipDistance = 5.0;
growParam.yDeviation_max = 10.0;
growParam.maxSkipDistance = 10.0;
growParam.zDeviation_max = 5.0;//
growParam.minLTypeTreeLen = 100; //mm
growParam.minVTypeTreeLen = 100; //mm
@ -831,8 +837,8 @@ int main()
workpieceParam.lineLen = 120; // 120.0; //直线段长度
else
workpieceParam.lineLen = 160.0; //直线段长度
workpieceParam.dirAngleScale = 20.0; //计算方向角的尺度
workpieceParam.lineDeviation = 10.0;//直线偏离度。工作直线边缘由于扫描和切割导致波动，此参数用于描述此波动。
workpieceParam.dirAngleScale = 25.0; //计算方向角的尺度
workpieceParam.lineDeviation = 20.0;//直线偏离度。工作直线边缘由于扫描和切割导致波动，此参数用于描述此波动。
workpieceParam.minCutAngleTh = 15; //最小截角。由于扫描不全导致的截角，当截角过小时无法区分真下的截角和由于点波动导致的角度波动
int errCode = 0;

1
BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.vcxproj
View File

@ -24,6 +24,7 @@
<ProjectGuid>{cf563709-0402-447e-bfcc-7701cc90d0af}</ProjectGuid>
<RootNamespace>BQworkpieceCornerExtracttest</RootNamespace>
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
<ProjectName>BQ_workpieceCornerExtraction_test</ProjectName>
</PropertyGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">

2
BQ_workpieceCornerExtraction/BQ_workpieceCornerExtraction.vcxproj
View File

@ -48,7 +48,7 @@
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
<ConfigurationType>DynamicLibrary</ConfigurationType>
<UseDebugLibraries>false</UseDebugLibraries>
<PlatformToolset>v142</PlatformToolset>
<PlatformToolset>v143</PlatformToolset>
<WholeProgramOptimization>true</WholeProgramOptimization>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>

13
beltTearingDetection_test/beltTearingDetection_test.cpp
View File

@ -942,7 +942,7 @@ void _outputObjInfo(char* fileName, std::vector<SSG_beltTearingInfo>& total_tear
sw.close();
}
#define TEST_GROUP 3
#define TEST_GROUP 4
int main()
{
#if 0
@ -1043,13 +1043,14 @@ int main()
#else
const char* dataPath[TEST_GROUP] = {
"F:\\ShangGu\\皮带撕裂点云\\1-500-2000\\", //0
"F:\\ShangGu\\皮带撕裂点云\\2-1700-2000\\", //1
"F:\\ShangGu\\皮带撕裂点云\\SaveData\\"
"F:/ShangGu/皮带撕裂点云/1-500-2000/", //0
"F:/ShangGu/皮带撕裂点云/2-1700-2000/", //1
"F:/ShangGu/皮带撕裂点云/SaveData/", //2
"F:/ShangGu/皮带撕裂点云/20260316_中信重工/", //3
};
SVzNLRange fileIdx[TEST_GROUP] = {
{10,24}, {2, 18}, {16,16} };
{10,24}, {2, 18}, {16,16}, {1,2} };
double camPoseR[9] = {
1.0, 0.0, 0.0,
@ -1067,7 +1068,7 @@ int main()
char _scan_file[256];
int endGroup = TEST_GROUP - 1;
for (int grp = 2; grp <= endGroup; grp++)
for (int grp = 3; grp <= endGroup; grp++)
{
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
{

1285
sourceCode/BQ_workpieceCornerExtraction.cpp
View File

File diff suppressed because it is too large Load Diff

11
sourceCode/BQ_workpieceCornerExtraction_Export.h
View File

@ -8,6 +8,9 @@
typedef struct
{
double lineLen; //直线段长度
double dirAngleScale; //计算方向角的尺度
double lineDeviation;//直线偏离度。工作直线边缘由于扫描和切割导致波动，此参数用于描述此波动。
double minCutAngleTh; //最小截角。由于扫描不全导致的截角，当截角过小时无法区分真下的截角和由于点波动导致的角度波动
}SSX_BQworkpiecePara;
typedef struct
@ -53,12 +56,12 @@ SG_APISHARED_EXPORT void sx_BQ_lineDataR(
//提取工件角点及定位长度信息
SG_APISHARED_EXPORT SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
std::vector< std::vector<SVzNL3DPosition>>& scanLines_src,
const SSG_cornerParam cornerPara,
const SSG_outlierFilterParam filterParam,
SSG_treeGrowParam growParam,
SSG_planeCalibPara groundCalibPara,
SSX_BQworkpiecePara workpieceParam,
const SSG_treeGrowParam growParam,
const SSG_planeCalibPara groundCalibPara,
const SSX_BQworkpiecePara workpieceParam,
#if _OUTPUT_DEBUG_DATA
std::vector<SSX_debugInfo>& debug_contours,
#endif

15
sourceCode/SG_baseAlgo_Export.h
View File

@ -677,6 +677,14 @@ SG_APISHARED_EXPORT void sg_pointClustering(
double clusterDist,
std::vector<std::vector< SVzNL3DPosition>>& objClusters //result
);
//使用SVzNL3DPosition的nPointIdx表示2D信息（高16位Line， 低16位ptIdx）
//搜索时搜索邻域以加速
SG_APISHARED_EXPORT void wd_pointClustering_speedUp(
std::vector< SVzNL3DPosition>& pts,
int lineNum, int ptSize, int clusterCheckWin, //搜索窗口
double clusterDist,
std::vector<std::vector< SVzNL3DPosition>>& objClusters //result
);
//基于栅格上点的窗口内的相邻点的聚类，聚类条件由3D点的邻域决定
//使用vector构成2维结构体数组
@ -760,6 +768,13 @@ SG_APISHARED_EXPORT void wd_noiseFilter(
const SSG_outlierFilterParam filterParam,
int* errCode);
//点云填孔平滑处理
SG_APISHARED_EXPORT void wd_pointCloudHoleFilling(
std::vector< std::vector<SVzNL3DPosition>>& scanLines_src,
std::vector< std::vector<SVzNL3DPosition>>& scanLines_dst,
const int fillingHoleSize //填孔大小
);
// Eigen库实现：计算从pts1向pts2的旋转平移矩阵
//需要
SG_APISHARED_EXPORT void caculateRT(

130
sourceCode/SG_clustering.cpp
View File

@ -9,6 +9,7 @@
void _seedClustering(
std::vector< SVzNL3DPosition>& a_cluster,
std::vector< SVzNL3DPosition>& pts,
std::vector<int> flags,
double clusterDist)
{
int i = 0;
@ -18,15 +19,61 @@ void _seedClustering(
break;
SVzNL3DPosition a_seed = a_cluster[i];
for (int i = 0, i_max = (int)pts.size(); i < i_max; i++)
for (int j = 0, j_max = (int)pts.size(); j < j_max; j++)
{
if (pts[i].nPointIdx < 0)
if (flags[j] < 0)
continue;
double dist = sqrt(pow(a_seed.pt3D.x - pts[i].pt3D.x, 2) + pow(a_seed.pt3D.y - pts[i].pt3D.y, 2));
if (dist < clusterDist)
double dist_x = abs(a_seed.pt3D.x - pts[j].pt3D.x);
double dist_y = abs(a_seed.pt3D.y - pts[j].pt3D.y);
if ( (dist_x < clusterDist) && (dist_y < clusterDist))
{
a_cluster.push_back(pts[i]);
pts[i].nPointIdx = -1;
a_cluster.push_back(pts[j]);
flags[j] = -1;
}
}
i++;
}
return;
}
void _seedClustering_speedUp(
std::vector< SVzNL3DPosition>& a_cluster,
std::vector< SVzNL3DPosition>& pts,
std::vector<int>& flags,
double clusterDist,
std::vector<std::vector<int>>& indexing2D,
int lineNum, int ptSize, int clusterCheckWin) //搜索窗口
{
int i = 0;
while (1)
{
if (i >= a_cluster.size())
break;
SVzNL3DPosition a_seed = a_cluster[i];
int seed_line = a_seed.nPointIdx >> 16;
int seed_ptIdx =a_seed.nPointIdx & 0x0000FFFF;
for (int line = seed_line - clusterCheckWin; line <= seed_line + clusterCheckWin; line++)
{
if ((line >= 0) && (line < lineNum))
{
for (int ptIdx = seed_ptIdx - clusterCheckWin; ptIdx <= seed_ptIdx + clusterCheckWin; ptIdx++)
{
if ((ptIdx >= 0) && (ptIdx < ptSize))
{
int backIdx = indexing2D[line][ptIdx];
if (backIdx < 0)
continue;
if (flags[backIdx] < 0)
continue;
double dist = sqrt(pow(a_seed.pt3D.x - pts[backIdx].pt3D.x,2) + pow(a_seed.pt3D.y - pts[backIdx].pt3D.y,2));
if (dist < clusterDist)
{
a_cluster.push_back(pts[backIdx]);
flags[backIdx] = -1;
}
}
}
}
}
i++;
@ -43,30 +90,95 @@ void sg_pointClustering(
int ptSize = (int)pts.size();
if (ptSize == 0)
return;
std::vector<int> flags;
flags.resize(ptSize);
std::fill(flags.begin(), flags.end(), 0);
while(pts.size() > 0)
{
SVzNL3DPosition a_pt = pts[0];
flags[0] = -1;//防止重复被计算
//新建一个cluster
std::vector< SVzNL3DPosition> a_cluster;
a_cluster.push_back(a_pt);
pts[0].nPointIdx = -1; //防止重复被计算
//pts[0].nPointIdx = -1;
_seedClustering(
a_cluster,
pts,
flags,
clusterDist);
objClusters.push_back(a_cluster); //保存一个聚类
//将pts中处理过的点去除
int m_max = (int)pts.size();
for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
{
if(pts[m].nPointIdx < 0)
if (flags[m] < 0)
{
pts.erase(pts.begin() + m);
flags.erase(flags.begin() + m);
}
}
}
return;
}
//使用SVzNL3DPosition的nPointIdx表示2D信息（高16位Line， 低16位ptIdx）
//搜索时搜索邻域以加速
void wd_pointClustering_speedUp(
std::vector< SVzNL3DPosition>& pts,
int lineNum, int linePtSize, int clusterCheckWin, //搜索窗口
double clusterDist,
std::vector<std::vector< SVzNL3DPosition>>& objClusters //result
)
{
std::vector<std::vector<int>> indexing2D;
indexing2D.resize(lineNum);
for (int i = 0; i < lineNum; i++)
{
indexing2D[i].resize(linePtSize);
std::fill(indexing2D[i].begin(), indexing2D[i].end(), -1);
}
for (int i = 0; i < (int)pts.size(); i++)
{
int line = pts[i].nPointIdx >> 16;
int ptIdx = pts[i].nPointIdx & 0x0000FFFF;
indexing2D[line][ptIdx] = i;
}
int ptSize = (int)pts.size();
if (ptSize == 0)
return;
std::vector<int> flags;
flags.resize(ptSize);
std::fill(flags.begin(), flags.end(), 0);
int idx = 0;
while (idx < (int)pts.size())
{
SVzNL3DPosition a_pt = pts[idx];
if (flags[idx] >= 0)
{
flags[idx] = -1;//防止重复被计算
//新建一个cluster
std::vector< SVzNL3DPosition> a_cluster;
a_cluster.push_back(a_pt);
//pts[0].nPointIdx = -1;
_seedClustering_speedUp(
a_cluster,
pts,
flags,
clusterDist,
indexing2D,
lineNum, linePtSize, clusterCheckWin);
objClusters.push_back(a_cluster); //保存一个聚类
}
idx++;
}
return;
}
//对特征点的聚类
void wd_gridPointClustering(
std::vector<std::vector<SSG_featureClusteringInfo>>& featureMask,

112
sourceCode/WD_noiseFilter.cpp
View File

@ -60,3 +60,115 @@ void wd_noiseFilter(
}
return;
}
void wd_pointCloudHoleFilling(
std::vector< std::vector<SVzNL3DPosition>>& scanLines_src,
std::vector< std::vector<SVzNL3DPosition>>& scanLines_dst,
const int fillingHoleSize //填孔大小
)
{
int lineNum = (int)scanLines_src.size();
scanLines_dst.resize(lineNum);
for (int line = 0; line < lineNum; line++)
{
int ptNum = (int)scanLines_src[line].size();
scanLines_dst[line].resize(ptNum);
for (int i = 0; i < ptNum; i++)
{
scanLines_dst[line][i] = scanLines_src[line][i];
scanLines_src[line][i].nPointIdx = 0; //未填充
}
}
for (int line = 0; line < lineNum; line++)
{
int ptNum = (int)scanLines_src[line].size();
for (int i = 0; i < ptNum; i++)
{
SVzNL3DPosition& a_pt = scanLines_src[line][i];
if ( (a_pt.pt3D.z < 1e-4) && (a_pt.nPointIdx == 0)) //未填充的空白点
{
//检查是否需要填充
//扫描线方向
int j_top = -1;
for (int j = i - 1; j >= i - fillingHoleSize; j--)
{
if (j < 0)
break;
else if (scanLines_src[line][j].pt3D.z > 1e-4)
{
j_top = j;
break;
}
}
int j_btm = -1;
for (int j = i + 1; j <= i + fillingHoleSize; j++)
{
if (j >= ptNum)
break;
else if (scanLines_src[line][j].pt3D.z > 1e-4)
{
j_btm = j;
break;
}
}
if ((j_top >= 0) && (j_btm >= 0))
{
SVzNL3DPoint& top_pt = scanLines_src[line][j_top].pt3D;
SVzNL3DPoint& btm_pt = scanLines_src[line][j_btm].pt3D;
//插值
double dist = (double)(j_btm - j_top);
for (int j = j_top + 1; j <= j_btm - 1; j++)
{
double d = (double)(j - j_top);
double k2 = d / dist;
double k1 = 1.0 - k2;
scanLines_dst[line][j].pt3D = { top_pt.x * k1 + btm_pt.x * k2, top_pt.y * k1 + btm_pt.y * k2 , top_pt.z * k1 + btm_pt.z * k2 };
scanLines_src[line][j].nPointIdx = 1;
}
}
else
{
//检查水平方向
int line_left = -1;
for (int j = line - 1; j >= line - fillingHoleSize; j--)
{
if (j < 0)
break;
else if (scanLines_src[j][i].pt3D.z > 1e-4)
{
line_left = j;
break;
}
}
int line_right = -1;
for (int j = line + 1; j <= line + fillingHoleSize; j++)
{
if (j >= lineNum)
break;
else if (scanLines_src[j][i].pt3D.z > 1e-4)
{
line_right = j;
break;
}
}
if ((line_left >= 0) && (line_right >= 0))
{
SVzNL3DPoint& left_pt = scanLines_src[line_left][i].pt3D;
SVzNL3DPoint& right_pt = scanLines_src[line_right][i].pt3D;
//插值
double dist = (double)(line_right - line_left);
for (int j = line_left + 1; j <= line_right - 1; j++)
{
double d = (double)(j - line_left);
double k2 = d / dist;
double k1 = 1.0 - k2;
scanLines_dst[j][i].pt3D = { left_pt.x * k1 + right_pt.x * k2, left_pt.y * k1 + right_pt.y * k2 , left_pt.z * k1 + right_pt.z * k2 };
scanLines_src[j][i].nPointIdx = 1;
}
}
}
}
}
}
}