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

rodAndBarDetection version 1.2.6 :

Browse Source 
定位盘位姿计算由RANSAC调整为迭代拟合，测试精度是否有提高
This commit is contained in:
jerryzeng 2026-05-08 06:10:42 +08:00
parent 0f45631530
commit bf95cd6578
1 changed files with 92 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

129
sourceCode/rodAndBarDetection.cpp
View File

@ -14,7 +14,8 @@
//version 1.2.3 : 根据定向盘轮廓的直线段确定向右的向量
//version 1.2.4 : 根据定向盘左上点和右下点确定姿态向量
//version 1.2.5 : 添加筑裕钢筋焊缝提取API
std::string m_strVersion = "1.2.5";
//version 1.2.6 : 定位盘位姿计算由RANSAC调整为迭代拟合，测试精度是否有提高
std::string m_strVersion = "1.2.6";
const char* wd_rodAndBarDetectionVersion(void)
{
return m_strVersion.c_str();
@ -210,6 +211,31 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
return _r_pt;
}
void rodAarcFeatueDetection(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSG_cornerParam cornerPara,
const SSG_outlierFilterParam filterParam,
const double rodDiameter,
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++)
{
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;
wd_getRodArcFeature_peakCornerMethod(lineData, line, rodDiameter / 2, cornerPara, line_rodArcs);
arcFeatures.push_back(line_rodArcs);
}
return;
}
//计算螺杆端部中心点位姿
void sx_hexHeadScrewMeasure(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
@ -287,72 +313,98 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
}
}
std::vector<std::vector<SWD_segFeature>> arcFeatures;
for (int line = 0; line < lineNum; line++)
{
if (line == 329)
int kkk = 1;
std::vector<SVzNL3DPosition>& lineData = data_lines[line];
//滤波，滤除异常点
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
std::vector<SWD_segFeature> line_ringArcs;
int dataSize = (int)lineData.size();
SVzNLRangeD arcWidth;
arcWidth.min = rodDiameter / 2;
arcWidth.max = rodDiameter * 1.5;
//提取Arc特征
wd_getRingArcFeature(
lineData,
line, //当前扫描线序号
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures;
rodAarcFeatueDetection(
data_lines,
cornerPara,
arcWidth, //环宽度，以半径为基准，对应60度角
line_ringArcs //环
);
arcFeatures.push_back(line_ringArcs);
}
//特征生长
std::vector<SWD_segFeatureTree> growTrees;
wd_getSegFeatureGrowingTrees(
arcFeatures,
growTrees,
growParam);
filterParam,
rodDiameter,
arcFeatures);
if (growTrees.size() == 0)
//特征生长
std::vector<SWD_rodArcFeatureTree> rodArcTrees;
wd_getRodArcFeatureGrowingTrees(arcFeatures, rodArcTrees, growParam);
if (rodArcTrees.size() == 0)
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return;
}
int objNum = (int)growTrees.size();
int objNum = (int)rodArcTrees.size();
//置标志，用于debug
for (int i = 0; i < objNum; i++)
{
int nodeNum = (int)growTrees[i].treeNodes.size();
int nodeNum = (int)rodArcTrees[i].treeNodes.size();
for (int j = 0; j < nodeNum; j++)
{
int lineIdx, ptIdx;
if (false == isHorizonScan)
{
lineIdx = growTrees[i].treeNodes[j].lineIdx;
for (int m = growTrees[i].treeNodes[j].startPtIdx; m <= growTrees[i].treeNodes[j].endPtIdx; m++)
lineIdx = rodArcTrees[i].treeNodes[j].lineIdx;
for (int m = rodArcTrees[i].treeNodes[j].startPtIdx; m <= rodArcTrees[i].treeNodes[j].endPtIdx; m++)
{
ptIdx = m;
if( (m == rodArcTrees[i].treeNodes[j].startPtIdx) || (m == rodArcTrees[i].treeNodes[j].endPtIdx) ||
(m == rodArcTrees[i].treeNodes[j].peakPtIdx))
scanLines[lineIdx][ptIdx].nPointIdx = 2;
else
scanLines[lineIdx][ptIdx].nPointIdx = 1;
}
}
else
{
ptIdx = growTrees[i].treeNodes[j].lineIdx;
for (int m = growTrees[i].treeNodes[j].startPtIdx; m <= growTrees[i].treeNodes[j].endPtIdx; m++)
ptIdx = rodArcTrees[i].treeNodes[j].lineIdx;
for (int m = rodArcTrees[i].treeNodes[j].startPtIdx; m <= rodArcTrees[i].treeNodes[j].endPtIdx; m++)
{
lineIdx = m;
if ((m == rodArcTrees[i].treeNodes[j].startPtIdx) || (m == rodArcTrees[i].treeNodes[j].endPtIdx) ||
(m == rodArcTrees[i].treeNodes[j].peakPtIdx))
scanLines[lineIdx][ptIdx].nPointIdx = 2;
else
scanLines[lineIdx][ptIdx].nPointIdx = 1;
}
}
}
}
//确定目标
//计算ROI，z最小的生长树为目标
std::vector<SVzNL3DRangeD> objROIs;
for (int i = 0; i < objNum; i++)
{
// Initialize min and max values
// Calculate X, Y and Z ranges
SVzNL3DRangeD a_roi3D;
a_roi3D.xRange.min = DBL_MAX; a_roi3D.xRange.max = DBL_MIN;
a_roi3D.yRange.min = DBL_MAX; a_roi3D.yRange.max = DBL_MIN;
a_roi3D.zRange.min = DBL_MAX; a_roi3D.zRange.max = DBL_MIN;
int nodeNum = (int)rodArcTrees[i].treeNodes.size();
for (int j = 0; j < nodeNum; j++)
{
SVzNL3DPoint peak;
int lineIdx, ptIdx;
if (false == isHorizonScan)
{
int lineIdx = rodArcTrees[i].treeNodes[j].lineIdx;
int ptIdx = rodArcTrees[i].treeNodes[j].peakPtIdx;
SVzNL3DPoint peak = scanLines[lineIdx][ptIdx].pt3D;
}
else
{
int ptIdx = rodArcTrees[i].treeNodes[j].lineIdx;
int lineIdx = rodArcTrees[i].treeNodes[j].peakPtIdx;
SVzNL3DPoint peak = scanLines[lineIdx][ptIdx].pt3D;
}
a_roi3D.xRange.min = std::min(a_roi3D.xRange.min, peak.x);
a_roi3D.xRange.max = std::max(a_roi3D.xRange.max, peak.x);
a_roi3D.yRange.min = std::min(a_roi3D.yRange.min, peak.y);
a_roi3D.yRange.max = std::max(a_roi3D.yRange.max, peak.y);
a_roi3D.zRange.min = std::min(a_roi3D.zRange.min, peak.z);
a_roi3D.zRange.max = std::max(a_roi3D.zRange.max, peak.z);
}
}
#if 0
//逐个目标处理
for (int i = 0; i < objNum; i++)
{
@ -455,6 +507,7 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
//screwInfo[i].rotateAngle += 90;
}
}
#endif
return;
}
@ -998,7 +1051,7 @@ SSX_platePoseInfo sx_getLocationPlatePose(
}
//计算面参数: z = Ax + By + C
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
#if 1
#if 0
std::vector<cv::Point3f> out_inliers;
Plane res = ransacFitPlane( Points3ds, out_inliers );
if (res.C < 0)